ncgm <- new("TimeInvariantNormalPGM")
ZZ <- NULL

stopifnot(nlatent(ncgm) == 2,
          naction(ncgm) == 1,
          nsuccess(ncgm) == 0)

ncgmparam <- drawPMParam(ncgm,ZZ)

stopifnot(all.equal(dim(ncgmparam@Fmat),c(2,2)),
          all.equal(dim(ncgmparam@Bmat),c(1,2)),
          all.equal(dim(ncgmparam@Sigma),c(2,2)),
          all.equal(dim(ncgmparam@Amat),c(2,2)),
          all.equal(length(ncgmparam@mu),c(2)))

nlat5.0 <- drawInitialLatent(ncgm,5,ZZ,ncgmparam)

stopifnot(all.equal(dim(nlat5.0),c(5,2)))

prior <- lpriorLatent(ncgm,nlat5.0,ZZ,ncgmparam)

nlat5.1 <- advanceLatent(ncgm,ncgmparam,nlat5.0,
                         action=1,success=NULL,ZZ,time=1)

stopifnot(all.equal(dim(nlat5.0),dim(nlat5.1)))


llike <- llikeAdvanceLatent(ncgm,ncgmparam,nlat5.0,nlat5.1,
                            action=1,success=NULL,ZZ,time=1)


Q1 <- matrix(c(1,0,0,1,1,1),nrow=3,byrow=TRUE)
nq <- sum(abs(Q1)>0)

stopifnot(all.equal(scaleQ(Q1),
                    (matrix(sqrt(c(1,0,0,1,1/2,1/2)),nrow=3,byrow=TRUE))))

nem <- new("FixedQNormalEM",
           hMean = rep(1,nq), hStd = rep(.5,nq),
           zMean = rep(5,nrow(Q1)), zStd = rep(1,nrow(Q1)),
           Rmean = diag(rep(2,nrow(Q1)))+.1,
           Rdf=5, Qmat=Q1)

stopifnot(nobserved(nem) == 3)

nemparam <- drawEMParam(nem, Q1,ZZ)

stopifnot(all.equal(getQ(nemparam),
                    (matrix(sqrt(c(1,0,0,1,1/2,1/2)),nrow=3,byrow=TRUE))))

stopifnot(length(nemparam@Hvec)==nq,
          all.equal(dim(getH(nemparam)),dim(Q1)),
          all(getH(nemparam)[Q1==0] == 0))

stopifnot(all.equal(dim(getR(nemparam)),rep(nobserved(nem),2)),
          length(getz(nemparam))==nobserved(nem))

## Test Setter methods

nemparam1 <- nemparam

getQ(nemparam1) <- matrix(c(1,0,0,1,3,4),nrow=3,byrow=TRUE)
stopifnot(all.equal(getQ(nemparam1),
                    matrix(c(1,0,0,1,3/5,4/5),nrow=3,byrow=TRUE)))

getH(nemparam1) <- matrix(1:6,nrow=3,byrow=TRUE)
## Data set byrow, but stored by column.
stopifnot(all.equal(nemparam1@Hvec,c(1,5,4,6)),
          all.equal(getH(nemparam1),matrix(c(1,0,0,4,5,6),nrow=3,byrow=TRUE)))

getR(nemparam1) <- diag(rep(1,3))
stopifnot (all.equal(nemparam1@Rmat,diag(rep(1,3))))

getz(nemparam1) <- 1:3
stopifnot (all.equal(nemparam1@z,1:3))


### Now test EM methods

obs1 <- drawObs(nem,nemparam,nlat5.0,Q1,ZZ)
stopifnot(all.equal(dim(obs1),c(5,3)))

lldat <- llikeObs(nem,nemparam,obs1,nlat5.0,Q1,ZZ)