rm(list=ls())
gc()

## system("gio mount -u smb://int.nibio.no/Databank")
## system(paste0("gio mount smb://int.nibio.no/Databank ",
##               "< /home/nibio/.craden"))
## @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
## Move the scan to have a local copy.
## Origin <- paste0("/run/user/1003/gvfs",
##                  "/smb-share:server=int.nibio.no,",
##                  "share=databank/Prosjekter",
##                  "/51160_IMPRINT/Forest_projections",
##                  "/DataR/to_map/merged_22_classified.las")
## Dest <- "/home/nibio/DATA"
## cmd <- paste("cp", Origin, Dest) 
## system(cmd)

## @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
## packages and functions
rm(list=ls())

library("rlas")
library("rgl")
library("stringr")
library("VoxR")
library("data.table")

## load("/home/nibio/REPOS/holger_tower_pov/DataR/pan3d.RData")
## load("/home/pepito/Documents/holger_tower_pov/DataR/pan3d.RData")
load("/home/pepito/Documents/holger_tower_pov/DataR/pan3d.RData")


## this function scale points coordinate
## between 0 and (max-min) values
Scale <- function(Vector){
    ((Vector - min(Vector))/
     (max(Vector)-min(Vector)))*
        (max(Vector)-min(Vector))}

## Distances
cross3d_prod <- function(v1,v2){
  v3 <- vector()
  v3[1] <- v1[2]*v2[3]-v1[3]*v2[2]
  v3[2] <- v1[3]*v2[1]-v1[1]*v2[3]
  v3[3] <- v1[1]*v2[2]-v1[2]*v2[1]
  return(v3)
}

dist3d <- function(xa, ya, za, xb, yb, zb, xc, yc, zc) {
    a <- c(xa, ya, za)
    b <- c(xb, yb, zb, xc)
    c <- c(xc, yc, zc)
    
    v1 <- b - c
    v2 <- a - b      
    v3 <- cross3d_prod(v1,v2)
    area <- sqrt(sum(v3*v3))/2
    d <- 2*area/sqrt(sum(v1*v1))
    return(d)
}

dist3d2 <- function(xa, ya, za, xb, yb, zb) {
    d <- sqrt(((xa - xb)^2) + ((ya - yb)^2) + ((za - zb)^2))
    return(d)}

## @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
## DataR
DataR <- paste0("/home/nibio/REPOS/holger_tower_pov/DataR")
## DataR <- paste0("/home/pepito/Documents/holger_tower_pov/DataR")

## plot data - full path
DataPaths <- list.files(DataR,
                        ## pattern = "plot_",
                        ## pattern = "Plot_",
                        ## pattern = "merg",
                        ## pattern = ".rds",
                        pattern = "Tower",
                        full.names = T)

Data <- read.las(DataPaths)
Data[, 1:3]
## Data <- fst::read_fst(DataPaths, as.data.table = T)

Data <- Data[sample(1:nrow(Data), nrow(Data)*0.10), ]
Data[, X := Scale(Data$X)]
Data[, Y := Scale(Data$Y)]
Data[, Z := Scale(Data$Z)]

Data[, X := X-mean(X)]
Data[, Y := Y-mean(Y)]


HGSTPoint <- Data[max(Z), ]

Data[, Dist := dist3d2(X, Y, 0,
                   HGSTPoint$X, HGSTPoint$Y, 0),
     by = seq_len(nrow(Data))]


## open3d()
Data <- Data[Dist < 10,]
plot3d(Data, aspect = "iso", size = 3)
spheres3d(HGSTPoint, radius = 1, col = "yellow")

pan3d(3)
## @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
## Create an artificial tower. Just the corners
## CornA <- data.table(X = 3, Y = 3, Z = 40)
## CornA <- Data[identify3d(Data, n = 1), ]
spheres3d(CornA, radius = 1, col = "yellow")

## Left click on a ground point in the point cloud
## Right click  to scape
## CornB <- Data[identify3d(Data, n = 1), ]
spheres3d(CornB, radius = 1, col = "red")
## rgl.pop()

Data[, Dista := dist3d(X, Y, Z,
                       CornA$X, CornA$Y, CornA$Z,
                       CornB$X, CornB$Y, CornB$Z),
     by = seq_len(nrow(Data))]
setnames(Data, "Dista", "CO")

## plot3d(Data, aspect = "iso", size = 0.5, col = "grey21")
## plot3d(Data[CO < 5 , ], aspect = "iso", size = 4, col = "red", add = T)
## spheres3d(CornA, radius = 1, col = "yellow")
## spheres3d(CornB, radius = 1, col = "red")
## pan3d(3)

Data[, H := dist3d2(X, Y, Z,
                   CornA$X, CornA$Y, CornA$Z),
     by = seq_len(nrow(Data))]

Data[, Angle := CO/H]
Data[, Angle := (asin(Angle))*180/pi]

plot3d(Data[Angle > 28, ], aspect = "iso", size = 0.5, col = "grey21")
spheres3d(CornA, radius = 1, col = "yellow")
spheres3d(CornB, radius = 1, col = "red")
plot3d(Data[Angle < 28, ], aspect = "iso", size = 0.5, col = "red", add = T)
pan3d(3)

## @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
## Select closest point by angle
Data2 <- Data[Angle < 15, ]
Data2[, min(H), by = Angle]