' ******************************************************************
' Test of Metropolis-Hastings algorithm
' ******************************************************************
' This program simulates a multinormal distribution by Markov Chain
' Monte Carlo (MCMC) using the Metropolis-Hastings algorithm.
'
' Although MCMC is best used when there is no direct way to simulate
' the distribution, it is used here for demonstration purposes since
' its results can be compared to those of the direct method (program
' RANMUL.BAS).
'
' The pdf P(X) of the multinormal distribution is such that:
'
'       P(X) = C * Exp(- F(X) / T)
'
' where F(X) = (X - M)' * V^(-1) * (X - M)
'
'       C = 1/sqrt(|V| * (2*Pi)^N)
'
'       T = 2
'
'       M is the mean vector and V the variance-covariance matrix of
'       the distribution. N is the dimension of the distribution.
'
'       The constant C is not used in the simulation.
'
' The results are stored in the output file TESTMCMC.TXT
' ******************************************************************

#include "fbmath.bi"

CONST Temp     = 2     ' Temperature
CONST NCycles  = 10    ' Number of cycles
CONST MaxSim   = 1000  ' Max nb of simulations
CONST SavedSim = 1000  ' Nb of saved simulations

DIM AS INTEGER N       ' Size of distribution
DIM AS INTEGER I, J    ' Loop variables
DIM AS DOUBLE  R       ' Correlation coefficient
DIM AS DOUBLE  Det     ' Determinant of var-cov. matrix

DATA 3                 ' Size of distribution

DATA 1, 0.1            ' Means and SD
DATA 2, 0.2
DATA 3, 0.3

DATA 0.25              ' Correlation matrix (lower triangular)
DATA 0.5, -0.25

READ N

DIM SHARED AS DOUBLE M(1 TO N)             ' Mean vector
DIM SHARED AS DOUBLE Vinv(1 TO N, 1 TO N)  ' Inverse of var-cov. mat.

DIM AS DOUBLE S(1 TO N)          ' Standard deviations
DIM AS DOUBLE V(1 TO N, 1 TO N)  ' Variance-covariance matrix

' Read means and standard deviations. Compute variances

FOR I = 1 TO N
  READ M(I), S(I)
  V(I, I) = S(I) * S(I)
NEXT I

' Read correlation coefficients and compute covariances

FOR I = 2 TO N
  FOR J = 1 TO I - 1
    READ R
    V(I, J) = R * S(I) * S(J)
    V(J, I) = V(I, J)
  NEXT J
NEXT I

' ******************************************************************
' Subprograms
' ******************************************************************

FUNCTION ObjFunc(X() AS DOUBLE) AS DOUBLE
' Computes the function F(X)

  DIM AS INTEGER I, J, Lb, Ub
  DIM AS DOUBLE  Sum1, Sum2

  Lb = LBOUND(X)
  Ub = UBOUND(X)

  DIM AS DOUBLE D(Lb TO Ub)

  FOR I = Lb TO Ub
    D(I) = X(I) - M(I)
  NEXT I

  Sum1 = 0
  FOR I = Lb TO Ub
    Sum1 = Sum1 + Vinv(I, I) * D(I) * D(I)
  NEXT I

  Sum2 = 0
  FOR I = Lb + 1 TO Ub
    FOR J = Lb TO I - 1
      Sum2 = Sum2 + Vinv(I, J) * D(I) * D(J)
    NEXT J
  NEXT I

  ObjFunc = Sum1 + 2 * Sum2
END FUNCTION

SUB WriteResults(Title AS STRING, M() AS DOUBLE, V() AS DOUBLE)

  DIM AS INTEGER I, J, Lb, Ub
  DIM AS DOUBLE  R

  Lb = LBOUND(M)
  Ub = UBOUND(M)

  DIM AS DOUBLE S(Lb TO Ub)

  PRINT
  PRINT
  PRINT Title; ":"
  PRINT

  PRINT "      Mean      S.D."
  PRINT "--------------------"

  FOR I = Lb TO Ub
    S(I) = SQR(V(I,I))
    PRINT USING "######.####"; M(I); S(I)
  NEXT I

  PRINT
  PRINT "Correlation matrix:"
  PRINT

  FOR I = Lb + 1 TO Ub
    FOR J = Lb TO I - 1
      R = V(I,J) / (S(I) * S(J))
      PRINT USING "######.####"; R;
    NEXT J
    PRINT
  NEXT I
END SUB

SUB WriteOutputFile(Title AS STRING, Xmat() AS DOUBLE)

  DIM AS INTEGER I, J, Lb, Ub1, Ub2

  Lb  = LBOUND(Xmat, 1)
  Ub1 = UBOUND(Xmat, 1)
  Ub2 = UBOUND(Xmat, 2)

  OPEN "testmcmc.txt" FOR OUTPUT AS #1

  PRINT #1, Title
  PRINT #1, " Iter";

  FOR J = Lb TO Ub2
    PRINT #1, "        X"; STR$(J); "  ";
  NEXT J

  PRINT #1, ""

  FOR I = Lb TO Ub1
    PRINT #1, USING "#####"; I;
    FOR J = Lb TO Ub2
      PRINT #1, USING " ######.####"; Xmat(I,J);
    NEXT J
    PRINT #1, ""
  NEXT I

  CLOSE #1
END SUB

' ******************************************************************
' Main program
' ******************************************************************

DIM AS DOUBLE Msim(1 TO N)
DIM AS DOUBLE X_min(1 TO N)
DIM AS DOUBLE Vsim(1 TO N, 1 TO N)
DIM AS DOUBLE Xmat(1 TO SavedSim, 1 TO N)
DIM AS DOUBLE F_min

FOR I = 1 TO N           ' Initialize inverse var-cov. matrix
  FOR J = 1 TO N
    Vinv(I, J) = V(I, J)
  NEXT J
NEXT I

GaussJordan Vinv(), Det  ' Compute inverse var-cov. matrix

IF MathErr <> 0 THEN
  PRINT "Singular variance-covariance matrix!"
  STOP
END IF

' Initialize the estimated mean vector and the variance-covariance
' matrix. For the sake of demonstration we start at a distance from
' the true mean and with enhanced standard deviations.

FOR I = 1 TO N
  Msim(I) = 3.0 * M(I)
  Vsim(I,I) = 10.0 * V(I,I)
NEXT I

' Perform Metropolis-Hastings simulations

InitMHParams NCycles, MaxSim, SavedSim

PRINT "Running. Please wait..."

Hastings @ObjFunc, Temp, Msim(), Vsim(), Xmat(), X_min(), F_min

IF MathErr = 0 THEN
  WriteResults "Original distribution", M(), V()
  WriteResults "Simulated distribution", Msim(), Vsim()
  WriteOutputFile "Metropolis-Hastings simulation", Xmat()
ELSE
  PRINT "Variance-covariance matrix is not positive definite!"
END IF

SLEEP
