#####
Null<-function(M) 
{
    tmp <- qr(M)
    set <- if (tmp$rank == 0) 
            1:nrow(M)  #modified from package MASS 
    else -(1:tmp$rank)
    qr.Q(tmp, complete = TRUE)[, set, drop = FALSE]
}


#####


#####
runif.stiefel<-function(R,m) {
  X<-matrix(rnorm(m*R),m,R)
  tmp<-eigen(t(X)%*%X)
  X%*%(   tmp$vec%*%sqrt(diag(1/tmp$val))%*%t(tmp$vec)  )
                             }

#####


#####
rW<-function(kap,m){
   .C("rW",kap=as.double(kap),m=as.integer(m),w=double(1))$w
                   }
##

##
rmf.vector<-function(kmu) {
  kap<-sqrt(sum(kmu^2)) ; mu<-kmu/kap ; m<-length(mu)
  if(kap==0) { u<-rnorm(length(kmu)) ; u<-u/sqrt(sum(u^2)) }
  if(kap>0) {
    if(m==1){ u<- (-1)^rbinom( 1,1,1/(1+exp(2*kap*mu))) }
  
    if(m>1) {
      W<-rW(kap,m)
      V<-rnorm(m-1) ;  V<-V/sqrt(sum(V^2))
      x<-c((1-W^2)^.5*t(V),W)
      u<-cbind( Null(mu),mu)%*%x
            }
          }
u}
#####


#####
rmf.matrix<-function(M) {
  svdM<-svd(M)
  H<-svdM$u%*%diag(svdM$d)
  m<-dim(H)[1] ; R<-dim(H)[2]

  ##
  cmet<-FALSE
  rej<-0
  while(!cmet) {
    U<-matrix(0,m,R)
    U[,1]<-rmf.vector(H[,1])

    lr<-0

    for(j in seq(2,R,length=R-1)) {

      N<-Null(U[,seq(1,j-1,length=j-1)])
      x<-rmf.vector(t(N)%*%H[,j])
      U[,j]<- N%*%x

      xn<- sqrt(sum( (t(N)%*%H[,j])^2))
      xd<- sqrt(sum( H[,j]^2 ))
      lbr<-  log(besselI(xn, .5*(m-j-1),expon.scaled=TRUE))-
             log(besselI(xd, .5*(m-j-1),expon.scaled=TRUE))
      if(is.na(lbr)){lbr<- .5*(log(xd) - log(xn)) }
      lr<- lr+ lbr + (xn-xd) + .5*(m-j-1)*( log(xd)-log(xn) )
                           }

    cmet<- (log(runif(1)) <  lr ) ; rej<-rej+(1-1*cmet)
                  }
  #list(U=U%*%t(svd(M)$v),rej=rej)                            
  U%*%t(svd(M)$v)
                          }
#####





rmf.matrix.gibbs<-function(M,X) {

  sM<-svd(M) 
  H<-sM$u%*%diag(sM$d)
  Y<-X%*%sM$v

  m<-dim(H)[1] ; R<-dim(H)[2]
  for(r in sample(1:R)) {
    N<-Null(Y[,-r])
    y<-rmf.vector(t(N)%*%H[,r])
    Y[,r]<- N%*%y
                         } 
  Y%*%t(sM$v)                     }  
 


###
rbmf.vector.gibbs<-function(A,c,x){
      
      evdA<-eigen(A)
      E<-evdA$vec
      l<-evdA$val

      y<-abs(t(E)%*%x)
      s<-sign(t(E)%*%x)
      d<-t(E)%*%c

      for(i in sample(1:length(x)) ) {
        q<-y^2/(1-y[i]^2)
        th<-rtheta( .5*(dim(E)[1]-3),(l[i]-t(q[-i])%*%l[-i]),
                        sum(s[-i]*(q[-i]^.5)*d[-i]),d[i] )
        y[i]<-sqrt(th)
        y[-i]<-sqrt((1-th)*q[-i])
        s[i]<-(-1)^rbinom(1,1,1/(1+exp(2*sqrt(th)*d[i])) )

                                     }
      x<-E%*%(y*s)
      x/sqrt(sum(x^2))
                              }

#####


#####
rbmf.matrix.gibbs<-function(A,B,C,X){

  m<-dim(X)[1] ;  R<-dim(X)[2]
  if(m>R){ 
     for(r in sample( seq(1,R,length=R))) {
       N<-Null(X[,-r]) 
       An<-B[r,r]*t(N)%*%(A)%*%N ; cn<- t(N)%*%C[,r]
       X[,r]<-N%*%rbmf.vector.gibbs(An,cn,t(N)%*%X[,r])
                                           }
          }

   if(m==R){
     for(s in seq(1,R,length=R)) { 
       r<-sample(seq(1,R,length=R),2)
       N<-Null( X[,-r]  )
       An<- t(N)%*%A%*%N
       Cn<- t(N)%*%C[,r]
       X[,r]<-N%*%rbmf.O2(An,B[r,r],Cn)
                                 }
           } 

  X }

#####
rtheta<-function(k,a,b,c) {

  d1<-1/2 ; d2<- 1+min(k, max( k-a,-1/2) )  
  lmx<-0
  if(a> k - d2 +1 ) { lmx<-  (k-d2+1)*(log(k-d2+1)-log(a))+ a-(k-d2+1) }
  
  bb<-max(0,b)
  thmx<-  c^2/(c^2+bb^2)  ; if( abs(c)==0 & bb==0) { thmx<-1 }
  
  lfg.mx  <- lmx +  
             (b*sqrt(1-thmx)+abs(c)*sqrt(thmx)) +
             log(1+exp(-2*abs(c)))

  cmet<-FALSE
  while(!cmet) {

    th<-rbeta(1,d1,d2)
    lfg.th <- (k-d2+1)*log(1-th)+a*th + 
              (b*sqrt(1-th)+abs(c)*sqrt(th)) + 
              log(1+exp(-2*sqrt(th)*abs(c))) 

    cmet<-( log(runif(1)) < lfg.th - lfg.mx )

                 }
  th                          }
#####


#####
ldbing.O2<-function(Z,A,B,C){ sum( diag( t(C)%*%Z + B%*%t(Z)%*%A%*%Z ) ) }

theta<-seq(0,2*pi,length=100)
O2<-array(dim=c(2,2,2*length(theta)))
O2[1,1,]<-c(cos(theta),cos(theta))
O2[2,1,]<-c(sin(theta),sin(theta))
O2[1,2,]<-c(sin(theta),-sin(theta))
O2[2,2,]<-c(-cos(theta),cos(theta))
rm(theta)


rbmf.O2<-function(A,B,C) {
  lpz<- apply(O2,3,ldbing.O2,A=A,B=B,C=C)
  O2[,,sample(1:length(lpz),1,prob=exp(lpz-max(lpz))) ]
                         }
#####