library(rgl)

mydnorm <- function(xs,
                    mu = matrix(0,nrow(xs),1),
                    sigma = diag(1,nrow(xs))) {
  if (missing(xs)) {
    cat(" mydnorm(xs, mu, sigma)\n",
        "    xs: nDimensions x nSamples\n",
        "    mu: nDimensions x 1\n",
        " sigma: nDimensions x nDimensions\n")
    cat(" Example: 
    > samples <- matrix(seq(0,1,length=15),3,5)
    > samples
               [,1]      [,2]      [,3]      [,4]      [,5]
    [1,] 0.00000000 0.2142857 0.4285714 0.6428571 0.8571429
    [2,] 0.07142857 0.2857143 0.5000000 0.7142857 0.9285714
    [3,] 0.14285714 0.3571429 0.5714286 0.7857143 1.0000000
    > mu <- matrix(c(0.2,0.3,0.1))
    > mu
         [,1]
    [1,]  0.2
    [2,]  0.3
    [3,]  0.1
    > sigma <- matrix(c(1,0.2,0,0.2,1,0,0,0,0.5),3,3)
    > sigma
         [,1] [,2] [,3]
    [1,]  1.0  0.2  0.0
    [2,]  0.2  1.0  0.0
    [3,]  0.0  0.0  0.5
    > mydnorm(samples, mu, sigma)
    [1] 0.04224973 0.04116455 0.03389237 0.02358084 0.01386423\n")

    return()
  }
  xs <- as.matrix(xs)                   # to handle args like c(0,0,0)
  nDimensions <- nrow(xs)                         # or dim(xs)[1]
  nSamples <- ncol(xs)

  normConstant <- 1/(2*pi)^(nDimensions/2) * sqrt(det(sigma))
  invSigma <- solve(sigma)
  xsZeroMean <- xs - matrix(rep(mu,nSamples),nDimensions,nSamples)

  normConstant * exp(-1/2 * colSums( t(t(xsZeroMean) %*% invSigma) * xsZeroMean ))

  
}

xs <- seq(0,10,len=40)
ys <- seq(5,15,len=40)
points <- t(expand.grid(xs,ys))

mean1 <- matrix(c(2,7))
sigma1 <- matrix(c(1,0,0,1),2,2)
mean2 <- matrix(c(7,11))
sigma2 <- matrix(c(3,2,2,4),2,2)
mean3 <- matrix(c(3,12))
sigma3 <- matrix(c(0.5,0.2,0.2,0.8),2,2)

g1<- matrix(0,nrow(points))
g2<- matrix(0,nrow(points))
g3<- matrix(0,nrow(points))
#for (i in 1:nrow(points)) {
  g1 <- mydnorm(points,mean1,sigma1)
  g2 <- mydnorm(points,mean2,sigma2)
  g3 <- mydnorm(points,mean3,sigma3)
#}

### First render the 3 Gaussians using RGL.

open3d()
view3d(theta=-20,phi=-90)
bg3d("white")
#material3d(color="black")
persp3d(xs,ys,g1,col="red",xlab="x",ylab="y",zlab="p(x|Ck)",aspect=c(1,1,0.4))
persp3d(xs,ys,g2,col="green", add=TRUE)
persp3d(xs,ys,g3,col=rgb(0.5,0.5,1.0), add=TRUE)

return()
rgl.postscript("rgltest.ps")
print("RGL figure saved in rgltest.ps")

### Now render using persp() and animate the rotation for 20 steps.

x11(type="Xlib")  ## x11() creates window of type cairo, by default, which is
                  ## a little slower to redraw

gaussiansInRows <- matrix(c(g1,g2,g3),nrow=3,byrow=TRUE)
maxGaussianValue <- apply(gaussiansInRows,2,max)
whichGaussianIsMax <- apply(gaussiansInRows,2,which.max)

for (theta in seq(0,360+60,len=20)) {
  persp(xs,ys,matrix(maxGaussianValue,length(xs),length(ys)),
        col=matrix(whichGaussianIsMax+1,length(xs),
          length(ys))[1:(length(xs)-1),1:(length(ys)-1)] ,
        xlab="x",ylab="y",zlab="p(x|Ck)",
        theta=theta, ticktype="detailed",
        cex=1.2)
  ## Call unix sleep command to slow the animation
  system("sleep 0.1")
}