library("geomorph")
library("shapes")
library("rgl")
# ### "Triangles of interest" on Kendall's sphere of radius 0.5 (=S^2(0.5)) in Cartesian coordinates
# 16 triangles located on the same hemisphere; 4 times 4 triangles with same latitude
tris_cartesian = rbind(c(0.4999998,    0, 0.0004999999),
                       c(0.4619398,    0, 0.1913417162),
                       c(0.3535534,    0, 0.3535533906),
                       c(0.1913417,    0, 0.4619397663),
                       c(-0.4999998,    0, 0.0004999999),
                       c(-0.4619398,    0, 0.1913417162),
                       c(-0.3535534,    0, 0.3535533906),
                       c(-0.1913417,    0, 0.4619397663))
tris_cartesian = rbind( tris_cartesian, cbind(tris_cartesian[,2], tris_cartesian[,1], tris_cartesian[,3]))

# function takes Cartesian coordinates of triangles on S^2(0.5) (row-vectors; nx3-matrix) and returns the point coordinates of  
# corresponding triangle ( = row vector with 6 components) such that a nx6-matrix is returned
# implemented following Kendall (1984) 
CoordiantesFromSphereToPoints = function(t_cartesian) { 
  # ## Calculate the matrix on p. 97 in Kendall (1984)
  m = rbind(c(1/sqrt(3), 1/sqrt(3), 1/sqrt(3)), 
            c(-(1/sqrt(2)), 1/sqrt(2), 0), 
            c(-(1/sqrt(6)), -(1/sqrt(6)), sqrt(2)/sqrt(3)))
  
  triangels_i = c()
  
  for (i in 1:nrow(t_cartesian ) ) {    # 
    # ##Use cartesian corrdiantes on S^2(0.5) to calculate Phi, theta and r in Kendall (1984) p. 97 
    theta = acos(2*t_cartesian[i, 1])
    r = tan(0.5 * theta)
    
    tmp = 2*t_cartesian[i, 2] / sin(theta)
    if (round(tmp, digits = 10) == 1) {
      Phi = 0
    } else if (sin(theta) ==0) {
      Phi = pi/2
    } else {
      Phi = acos(  2*t_cartesian[i, 2] / sin(theta)   )
    }
    # ## Calculate points of triangles according to Kendall p. 97
    m1 = rbind(c(0, 1, r*cos(Phi)), 
               c(0, 0, r*sin(Phi)))
    
    t= m1 %*% m
    t = matrix(t, nrow = 1, ncol = 6, byrow = F)
    triangels_i = rbind(triangels_i, t)
  }
  return(triangels_i)
}
# get triangles (with 6 coordinates) from the 3 Cartesian coordinates on S^2(0.5) 
tris = CoordiantesFromSphereToPoints(tris_cartesian)


# Lots of equilateral triangles to enforce the correct pole for tangent space projection
n_tmp = 10000
tris_tmp = matrix(rep( c(-1, 0,   1, 0,   0, sqrt(3)),  n_tmp), nrow = n_tmp, ncol = 6, byrow = T)

# Combine and reformat triangles  
tris_tmp = rbind(c(-1, 0,   1, 0,   0, sqrt(3)), tris, tris_tmp)  
tris_tmp = arrayspecs(tris_tmp, p = 3, k = 2)

# ### GPA with and without tangent space projection
gpa = gpagen(tris_tmp, PrinAxes = F, Proj = F)
# test, if pole is correct
riemdist(gpa$consensus, rbind(c(-1, 0),   c(1, 0),   c(0, sqrt(3))))

gpa_tangent = gpagen(tris_tmp, PrinAxes = F, Proj = T)
# test, if pole is correct
riemdist(gpa_tangent$consensus, rbind(c(-1, 0),   c(1, 0),   c(0, sqrt(3))))

# ### PCA and visualization
pca = gm.prcomp(gpa$coords)
plot3d(pca$x[ , 1], pca$x[ , 2], pca$x[ , 3], aspect = "iso")
spheres3d(pca$x[1:17 , 1], pca$x[1:17 , 2], pca$x[1:17 , 3], col = "red", radius =0.01 )
text3d(pca$x[1:17 , 1], pca$x[1:17 , 2], pca$x[1:17 , 3], text=c(1:17), cex = 2, adj = 2)


# ####### Compare different distance measures for "triangles of interest"
# aligned "triangles of interest"
tris = gpa$coords[,,1:17]
tris_tangent= gpa_tangent$coords[,,1:17]

# ### Different distances from pole
dists_fromPole = c()
for (i in 2:17) {
  # Procrustes Distance = distance on sphere with radius 0.5
  d_proc = riemdist(tris[,, 1], tris[,,i])
  
  # arc length on hemisphere of radius 1 (coordinates from GPA)
  tmp = as.vector(t(tris[,,1]) )
  tmp1 = as.vector(t(tris[,,i]) )
  arc_length_hemisphere = acos( sum(tmp * tmp1) / (sqrt(sum(tmp^2)) * sqrt(sum(tmp1^2))) )
  
  # Euclidean distance "on hemisphere" between coordinates from partial GPA
  eucl_dist_hemisphere = sqrt(sum( (tmp - tmp1)^2 ))
  
  # Euclidean distance in tangent space
  tmp = as.vector(t(tris_tangent[,,1]) )
  tmp1 = as.vector(t(tris_tangent[,,i]) )
  eucl_dist_tangent = sqrt(sum( (tmp - tmp1)^2 ))
  
  dists_fromPole = rbind(dists_fromPole, c(paste0("1-", i), d_proc, arc_length_hemisphere, eucl_dist_hemisphere, eucl_dist_tangent))
}
dimnames(dists_fromPole)[[2]] = c("dist_between", "ProcDist", "arcLengthHemisphere", "EuclDistHemisphere", "EuclDistTanget")

plot(dists_fromPole[ , "ProcDist"], dists_fromPole[ , "ProcDist"], col = "green", xlab = "Procrustes distance", ylab = "different distance measures",
     main = "Triangles, distance from pole",
     ylim = c(min(as.numeric(dists_fromPole[ , 2:4])), max(as.numeric(dists_fromPole[ , 2:4]))))
points(dists_fromPole[ , "ProcDist"], dists_fromPole[ , "EuclDistHemisphere"], col = "blue")
points(dists_fromPole[ , "ProcDist"], dists_fromPole[ , "EuclDistTanget"], col = "red", pch = 3)
legend("bottomright", legend = c("ProcD", "EuclD", "EuclD_tangent"), col = c("green", "blue","red"), pch = c(1,1,3)) 

# ### Distances along longitude (for one "hemi-circle" only)
dists_longitude = c( )
inds = c(2:5, 6:9)
for (i in 1:(length(inds)-1)) {
  for (j in (i+1):length(inds)) {
    # Procrustes Distance = distance on sphere with radius 0.5
    d_proc = riemdist(tris[,, inds[i]], tris[,,inds[j]])
    
    # arc length on hemisphere of radius 1 (coordinates from GPA)
    tmp = as.vector(t(tris[,,inds[i]]) )
    tmp1 = as.vector(t(tris[,,inds[j]]) )
    arc_length_hemisphere = acos( sum(tmp * tmp1) / (sqrt(sum(tmp^2)) * sqrt(sum(tmp1^2))) )
    
    # Euclidean distance "on hemisphere" between coordinates from partial GPA
    eucl_dist_hemisphere = sqrt(sum( (tmp - tmp1)^2 ))
    
    # Euclidean distance in tangent space
    tmp = as.vector(t(tris_tangent[,,inds[i]]) )
    tmp1 = as.vector(t(tris_tangent[,,inds[j]]) )
    eucl_dist_tangent = sqrt(sum( (tmp - tmp1)^2 ))
    
    dists_longitude = rbind(dists_longitude, c(paste0(inds[i], "-", inds[j]), d_proc, arc_length_hemisphere, eucl_dist_hemisphere, eucl_dist_tangent))
  }
}
dimnames(dists_longitude)[[2]] = c("dist_between", "ProcDist", "arcLengthHemisphere", "EuclDistHemisphere", "EuclDistTanget")

plot(dists_longitude[ , "ProcDist"], dists_longitude[ , "ProcDist"], col = "green", xlab = "Procrustes distance", ylab = "different distance measures",
     main = "Triangles, same longitude",
     ylim = c(min(as.numeric(dists_longitude[ , 2:4])), max(as.numeric(dists_longitude[ , 2:4]))))
points(dists_longitude[ , "ProcDist"], dists_longitude[ , "EuclDistHemisphere"], col = "blue")
points(dists_longitude[ , "ProcDist"], dists_longitude[ , "EuclDistTanget"], col = "red", pch = 3)
legend("bottomright", legend = c("ProcD", "EuclD", "EuclD_tangent"), col = c("green", "blue","red"), pch = c(1,1,3)) 

# ###  distances along latitude (only for two adjacent "quarter-circles")
dists_latitude = c()
inds = c(2:5)
inds1 = c(10:13)
for (i in 1:4) {
    # Procrustes Distance = distance on sphere with radius 0.5
    d_proc = riemdist(tris[,, inds[i]], tris[,,inds1[i]])
    
    # arc length on hemisphere of radius 1 (coordinates from GPA)
    tmp = as.vector(t(tris[,,inds[i]]) )
    tmp1 = as.vector(t(tris[,,inds1[i]]) )
    arc_length_hemisphere = acos( sum(tmp * tmp1) / (sqrt(sum(tmp^2)) * sqrt(sum(tmp1^2))) )
    
    # Euclidean distance "on hemisphere" between coordinates from partial GPA
    eucl_dist_hemisphere = sqrt(sum( (tmp - tmp1)^2 ))
    
    # Euclidean distance in tangent space
    tmp = as.vector(t(tris_tangent[,,inds[i]]) )
    tmp1 = as.vector(t(tris_tangent[,,inds1[i]]) )
    eucl_dist_tangent = sqrt(sum( (tmp - tmp1)^2 ))
    
    dists_latitude = rbind(dists_latitude, c(paste0(inds[i], "-", inds1[i]), d_proc, arc_length_hemisphere, eucl_dist_hemisphere, eucl_dist_tangent))
}
dimnames(dists_latitude)[[2]] = c("dist_between", "ProcDist", "arcLengthHemisphere", "EuclDistHemisphere", "EuclDistTanget")

plot(dists_latitude[ , "ProcDist"], dists_latitude[ , "ProcDist"], col = "green", xlab = "Procrustes distance", ylab = "different distance measures",
     main = "Triangles, same latitude",
     ylim = c(min(as.numeric(dists_latitude[ , 2:4])), max(as.numeric(dists_latitude[ , 2:4]))))
points(dists_latitude[ , "ProcDist"], dists_latitude[ , "EuclDistHemisphere"], col = "blue")
points(dists_latitude[ , "ProcDist"], dists_latitude[ , "EuclDistTanget"], col = "red", pch = 3)
legend("bottomright", legend = c("ProcD", "EuclD", "EuclD_tangent"), col = c("green", "blue","red"), pch = c(1,1,3)) 

# ###  distances neither on longitude nor on latitude (only for two adjacent "quarter-circles")
dists_diagonally = c()
inds = c(2:5)
inds1 = c(10:13)
for (i in 1:4) {
  for (j in inds1[-i]) {
    # Procrustes Distance = distance on sphere with radius 0.5
    d_proc = riemdist(tris[,, inds[i]], tris[,,j])
    
    # arc lenght on hemisphere of radius 1 (coordinates from GPA)
    tmp = as.vector(t(tris[,,inds[i]]) )
    tmp1 = as.vector(t(tris[,,j]) )
    arc_length_hemisphere = acos( sum(tmp * tmp1) / (sqrt(sum(tmp^2)) * sqrt(sum(tmp1^2))) )
    
    # Euclidean distance "on hemisphere" between coordinates from partial GPA
    eucl_dist_hemisphere = sqrt(sum( (tmp - tmp1)^2 ))
    
    # Euclidean distance in tangent space
    tmp = as.vector(t(tris_tangent[,,inds[i]]) )
    tmp1 = as.vector(t(tris_tangent[,,j]) )
    eucl_dist_tangent = sqrt(sum( (tmp - tmp1)^2 ))
    
    dists_diagonally = rbind(dists_diagonally, c(paste0(inds[i], "-", j), d_proc, arc_length_hemisphere, eucl_dist_hemisphere, eucl_dist_tangent))
  }
}
dimnames(dists_diagonally)[[2]] = c("dist_between", "ProcDist", "arcLengthHemisphere", "EuclDistHemisphere", "EuclDistTanget")

plot(dists_diagonally[ , "ProcDist"], dists_diagonally[ , "ProcDist"], col = "green", xlab = "Procrustes distance", ylab = "different distance measures",
     main = "Triangles, distance measured diagonally",
     ylim = c(min(as.numeric(dists_diagonally[ , 2:4])), max(as.numeric(dists_diagonally[ , 2:4]))))
points(dists_diagonally[ , "ProcDist"], dists_diagonally[ , "EuclDistHemisphere"], col = "blue")
points(dists_diagonally[ , "ProcDist"], dists_diagonally[ , "EuclDistTanget"], col = "red", pch = 3)
legend("bottomright", legend = c("ProcD", "EuclD", "EuclD_tangent"), col = c("green", "blue","red"), pch = c(1,1,3)) 

# Test for linear correlation of correct Procrustes distance with Euclidean distance on hemisphere or in tangent space
cor.test(as.numeric(dists_diagonally[ , "ProcDist"]), as.numeric(dists_diagonally[ , "EuclDistHemisphere"]), method = "pearson")
cor.test(as.numeric(dists_diagonally[ , "ProcDist"]), as.numeric(dists_diagonally[ , "EuclDistTanget"]), method = "pearson")


# ###  Compare distances for random triangles
n_rand_tis = 100
tris_rand = matrix(rnorm(n_rand_tis*6), nrow = n_rand_tis, ncol = 6)

# GPA without and with tangent space projection
tris_rand = arrayspecs(tris_rand, p = 3, k = 2)

gpa_rand = gpagen(tris_rand, PrinAxes = F, Proj = F)
gpa_rand_tangent = gpagen(tris_rand, PrinAxes = F, Proj = T)

tris_rand = gpa_rand$coords
tris_rand_tangent = gpa_rand_tangent$coords

dists_rand = c()
for (i in 1:(n_rand_tis-1) ) {
  for (j in (i+1):n_rand_tis ) {
    # Procrustes Distance = distance on sphere with radius 0.5
    d_proc = riemdist(tris_rand[,, i], tris_rand[,,j])
    
    # arc lenght on hemisphere of radius 1 (coordinates from GPA)
    tmp = as.vector(t(tris_rand[,,i]) )
    tmp1 = as.vector(t(tris_rand[,,j]) )
    arc_length_hemisphere = acos( sum(tmp * tmp1) / (sqrt(sum(tmp^2)) * sqrt(sum(tmp1^2))) )
    
    # Euclidean distance "on hemisphere" between coordinates from partial GPA
    eucl_dist_hemisphere = sqrt(sum( (tmp - tmp1)^2 ))
    
    # Euclidean distance in tangent space
    tmp = as.vector(t(tris_rand_tangent[,,i]) )
    tmp1 = as.vector(t(tris_rand_tangent[,,j]) )
    eucl_dist_tangent = sqrt(sum( (tmp - tmp1)^2 ))
    
    dists_rand = rbind(dists_rand, c(paste0(i, "-", j), d_proc, arc_length_hemisphere, eucl_dist_hemisphere, eucl_dist_tangent))
  }
}
dimnames(dists_rand)[[2]] = c("dist_between", "ProcDist", "arcLengthHemisphere", "EuclDistHemisphere", "EuclDistTanget")

plot(dists_rand[ , "ProcDist"], dists_rand[ , "ProcDist"], col = "green", xlab = "Procrustes distance", ylab = "different distance measures",
     main = "Random triangles, any direction",
     ylim = c(min(as.numeric(dists_rand[ , 2:4])), max(as.numeric(dists_rand[ , 2:4]))))
points(dists_rand[ , "ProcDist"], dists_rand[ , "EuclDistHemisphere"], col = "blue")
points(dists_rand[ , "ProcDist"], dists_rand[ , "EuclDistTanget"], col = "red", pch = 3)
legend("bottomright", legend = c("ProcD", "EuclD", "EuclD_tangent"), col = c("green", "blue","red"), pch = c(1,1,3)) 

# Test for correlation between Procrustes distance (= distance on sphere with radius 0.5 ) and Euclidean distance "on hemisphere"
corr = cor.test(as.numeric(dists_rand[ , "ProcDist"]), as.numeric(dists_rand[ , "EuclDistHemisphere"]), method = "pearson")
# Test for correlation between Procrustes distance (= distance on sphere with radius 0.5 ) and Euclidean distance in tangent space
corr_tangent = cor.test(as.numeric(dists_rand[ , "ProcDist"]), as.numeric(dists_rand[ , "EuclDistTanget"]), method = "pearson")

lm = lm(as.numeric(dists_rand[ , "EuclDistHemisphere"]) ~ as.numeric(dists_rand[ , "ProcDist"]))
lm_tangent = lm(as.numeric(dists_rand[ , "EuclDistTanget"]) ~ as.numeric(dists_rand[ , "ProcDist"]))


corr
corr_tangent
lm
lm_tangent



