/*
 * Copyright (C) 1996-2012 Daniel Waggoner and Tao Zha
 *
 * This free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * It is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * If you did not received a copy of the GNU General Public License
 * with this software, see <http://www.gnu.org/licenses/>.
 */

#include "dw_metropolis_block.h"
#include "dw_rand.h"
#include "dw_matrix_rand.h"
#include "dw_matrix_array.h"
#include "dw_std.h"

#include <stdlib.h>
#include <memory.h>
#include <math.h>
#include <time.h>

//-----------------------------------------------------------------------------//
//----------------------------- Metropolis Draws ------------------------------//
//-----------------------------------------------------------------------------//
TMetropolis_block* Create_metropolis_block(int n_parameters, void (*Get)(void*, PRECISION*), void (*Put)(void*, PRECISION*),PRECISION (*Density)(void*),PRECISION (*Gibbs)(PRECISION, void*))
{
  TMetropolis_block *metropolis=(TMetropolis_block*)dw_malloc(sizeof(TMetropolis_block));

  metropolis->n_parameters=n_parameters;

  metropolis->n_blocks=0;
  metropolis->block_dims=(int*)NULL;
  metropolis->block_offsets=(int*)NULL;
  metropolis->draws=(int*)NULL;
  metropolis->jumps=(int*)NULL;

  pGetParameters=Get;
  pPutParameters=Put;
  pDensity=Density;
  pDrawGibbs=Gibbs;

  InitializeVector(metropolis->scale=CreateVector(n_parameters),1.0);
  metropolis->direction=dw_CreateArray_vector(n_parameters);

  return metropolis;
}

void Free_metropolis_block(TMetropolis_block *metropolis)
{
  if (metropolis)
    {
      if (metropolis->block_dims) dw_free(metropolis->block_dims);
      if (metropolis->block_offsets) dw_free(metropolis->block_offsets);
      if (metropolis->draws) dw_free(metropolis->draws);
      if (metropolis->jumps) dw_free(metropolis->jumps);
      FreeVector(metropolis->scale);
      dw_FreeArray(metropolis->direction);
    }
}

/*
   Sets the metropolis scale and directions.
*/
void Setup_metropolis(TMetropolis_block *metropolis, TVector scale, TVector *direction)
{
  EquateVector(metropolis->scale,scale);
  dw_CopyArray(metropolis->direction,direction);
}

/*
   Sets the metropolis scale and directions.  The scales are set to one and the
   directions are set to null, which implies that the ith direction vector is the
   ith column of the identity matrix. 
*/
void Setup_metropolis_diagonal(TMetropolis_block *metropolis)
{
  int i;
  for (i=metropolis->n_parameters-1; i >= 0; i--)
    {
      ElementV(metropolis->scale,i)=1.0;
      if (metropolis->direction[i]) 
        {
          FreeVector(metropolis->direction[i]);
          metropolis->direction[i]=(TVector)NULL;
        }
    }
}

/*
   Sets the metropolis scale and directions.  The directions and scales are 
   computed from the eigenvalue-eigenvector decomposition of the hessian matrix.
   The matrix hessian is assumed to be n_parameters x n_parameters.
*/
void Setup_metropolis_hessian(TMetropolis_block *metropolis, TMatrix hessian)
{
  TMatrix U, V;
  TVector D;
  int i,j, n=metropolis->n_parameters;
  PRECISION ratio;

  if (!SVD(U=CreateMatrix(n,n),D=CreateVector(n),V=CreateMatrix(n,n),hessian))
    {
      printf("Error SetupMetropolisBlock_hessian(): unable to compute SVD\n");
      dw_exit(0);
    }

  for (i=0; i < n; i++)
    for (j=0; j < n; j++)
      {
        ratio=(fabs(ElementM(U,i,j)) > fabs(ElementM(V,i,j))) ? ElementM(V,i,j)/ElementM(U,i,j) : ((fabs(ElementM(V,i,j)) > 0.0) ? ElementM(U,i,j)/ElementM(V,i,j) : 1.0);
	if (ratio < 0.99)
	  {
	    printf("Error SetupMetropolisBlock_hessian(): hessian not symmetric and positive semi-definite\n");
	    printf("U(%d,%d) = %lf  V(%d,%d) = %lf\n  ratio = %le\n",i,j,ElementM(U,i,j),i,j,ElementM(V,i,j),ratio);
	    dw_exit(0);
	  }
      }

  if (ElementV(D,n-1) > SQRT_MACHINE_EPSILON*Element(D,0))
    for (i=0; i < n; i++)
      {
	ElementV(metropolis->scale,i) = (ElementV(D,n-1) > SQRT_MACHINE_EPSILON*ElementV(D,i)) ? 1.0/sqrt(ElementV(D,i)) : sqrt(SQRT_MACHINE_EPSILON/ElementM(D,n-1));
	metropolis->direction[i]=ColumnVector(metropolis->direction[i],U,i);
      }
  else
    for (i=0; i < n; i++)
      {
	ElementV(metropolis->scale,i) = (ElementV(D,i) > 1) ? 1.0 : sqrt(1.0/(ElementV(D,i)+MACHINE_EPSILON));
	metropolis->direction[i]=ColumnVector(metropolis->direction[i],U,i);
      }

  FreeMatrix(U);
  FreeMatrix(V);
  FreeVector(D);
}

/*
   Sets the metropolis scale and directions.  The directions and scales are 
   computed from the eigenvalue-eigenvector decomposition of the variance matrix.
   The matrix variance is assumed to be n_parameters x n_parameters.
*/
void Setup_metropolis_variance(TMetropolis_block *metropolis, TMatrix variance)
{
  TMatrix U, V;
  TVector D;
  int i,j, n=metropolis->n_parameters;
  PRECISION ratio;

  if (!SVD(U=CreateMatrix(n,n),D=CreateVector(n),V=CreateMatrix(n,n),variance))
    {
      printf("Error SetupMetropolisBlock_variance(): unable to compute SVD\n");
      dw_exit(0);
    }

  for (i=0; i < n; i++)
    for (j=0; j < n; j++)
      {
        ratio=(fabs(ElementM(U,i,j)) > fabs(ElementM(V,i,j))) ? ElementM(V,i,j)/ElementM(U,i,j) : ((fabs(ElementM(V,i,j)) > 0.0) ? ElementM(U,i,j)/ElementM(V,i,j) : 1.0);
        if (ratio < 0.99)
	  {
	    printf("Error SetupMetropolisBlock_variance(): variance not symmetric and positive definite\n");
	    printf("U(%d,%d) = %lf  V(%d,%d) = %lf\n  ratio = %le\n",i,j,ElementM(U,i,j),i,j,ElementM(V,i,j),ratio);
	    dw_exit(0);
	  }
      }

  if (ElementV(D,0) > 0)
    for (i=0; i < n; i++)
      {
	ElementV(metropolis->scale,i)=(ElementV(D,i) > SQRT_MACHINE_EPSILON*ElementV(D,0)) ? sqrt(ElementV(D,i)) : sqrt(ElementV(D,0)*SQRT_MACHINE_EPSILON);
	metropolis->direction[i]=ColumnVector(metropolis->direction[i],U,i);
      }
  else
    for (i=0; i < n; i++)
      {
	ElementV(metropolis->scale,i)=SQRT_MACHINE_EPSILON;
	metropolis->direction[i]=ColumnVector(metropolis->direction[i],U,i);
      }

  FreeMatrix(U);
  FreeMatrix(V);
  FreeVector(D);
}

/*
   Creates a block structure for metropolis draws.  The dimensions are given by 
   the integer array dims.  The array must be of length n_blocks and the sum of 
   the dimensions must be equal to n_parameters.
*/
void Setup_metropolis_blocks(TMetropolis_block *metropolis, int n_blocks, int *dims)
{
  int i;
  if (metropolis->block_dims) dw_free(metropolis->block_dims);
  if (metropolis->block_offsets) dw_free(metropolis->block_offsets);
  if (metropolis->draws) dw_free(metropolis->draws);
  if (metropolis->jumps) dw_free(metropolis->jumps);
 
  metropolis->n_blocks=n_blocks;
  metropolis->block_dims=(int*)dw_malloc(n_blocks*sizeof(int));
  metropolis->block_offsets=(int*)dw_malloc((n_blocks+1)*sizeof(int));
  metropolis->draws=(int*)dw_malloc(n_blocks*sizeof(int));
  metropolis->jumps=(int*)dw_malloc(n_blocks*sizeof(int));

  for (metropolis->block_offsets[0]=i=0; i < n_blocks; i++)
    {
      metropolis->draws[i]=metropolis->jumps[i]=0;
      metropolis->block_dims[i]=dims[i];
      metropolis->block_offsets[i+1]=metropolis->block_offsets[i]+dims[i];
    }

  if (metropolis->block_offsets[n_blocks] != metropolis->n_parameters)
    {
      printf("Error SetupMetropolisBlockBlocks(): invalid block dimensions\n");
      dw_exit(0);
    }
}

/*
   Creates a block structure for metropolis draws.  There are n_parameters 
   blocks, each of dimension one.
*/
void Setup_metropolis_full(TMetropolis_block *metropolis)
{
  int i;
  if (metropolis->block_dims) dw_free(metropolis->block_dims);
  if (metropolis->block_offsets) dw_free(metropolis->block_offsets);
  if (metropolis->draws) dw_free(metropolis->draws);
  if (metropolis->jumps) dw_free(metropolis->jumps);
 
  metropolis->n_blocks=metropolis->n_parameters;
  metropolis->block_dims=(int*)dw_malloc(metropolis->n_parameters*sizeof(int));
  metropolis->block_offsets=(int*)dw_malloc((metropolis->n_parameters + 1)*sizeof(int));
  metropolis->draws=(int*)dw_malloc(metropolis->n_parameters*sizeof(int));
  metropolis->jumps=(int*)dw_malloc(metropolis->n_parameters*sizeof(int));

  for (i=0; i < metropolis->n_parameters; i++)
    {
      metropolis->draws[i]=metropolis->jumps[i]=0;
      metropolis->block_dims[i]=1;
      metropolis->block_offsets[i]=i;
    }
  metropolis->block_offsets[metropolis->n_parameters]=metropolis->n_parameters;
}

/*
   Creates a block structure for metropolis draws.  There one block of dimension
   n_parameters.
*/
void Setup_metropolis_single(TMetropolis_block *metropolis)
{
  if (metropolis->block_dims) dw_free(metropolis->block_dims);
  if (metropolis->block_offsets) dw_free(metropolis->block_offsets);
  if (metropolis->draws) dw_free(metropolis->draws);
  if (metropolis->jumps) dw_free(metropolis->jumps);
 
  metropolis->n_blocks=1;
  metropolis->block_dims=(int*)dw_malloc(sizeof(int));
  metropolis->block_offsets=(int*)dw_malloc(2*sizeof(int));
  metropolis->draws=(int*)dw_malloc(sizeof(int));
  metropolis->jumps=(int*)dw_malloc(sizeof(int));

  metropolis->draws[0]=metropolis->jumps[0]=0;
  metropolis->block_offsets[0]=0;
  metropolis->block_offsets[1]=metropolis->block_dims[0]=metropolis->n_parameters;
}

/*
   Resets Metropolis counts.
*/
void ResetCounts_metropolis_block(TMetropolis_block *metropolis)
{
  int i;
  for (i=metropolis->n_blocks-1; i >= 0; i--)
    metropolis->draws[i]=metropolis->jumps[i]=0;
}


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/*
   Assumes:
     logdensity : log of the density kernel of the distribution evaluated at the current point
     metropolis : pointer to valid TMetropolis_block structure

   Results:
     A metropolis draw stored as the current point in metropolis->structure.  If the 
     metropolis draw is inside a gibbs draw, then the gibbs draw is also made.

   Returns:
     The log of the density kernel evaluated at the current point
*/
PRECISION draw_metropolis_in_gibbs(PRECISION logdensity, TMetropolis_block *metropolis)
{
  if (metropolis->pDrawGibbs)
    logdensity=metropolis->pDrawGibbs(logdensity,metropolis->structure);

  return metropolis_draw_all_blocks(logdensity,metropolis);
}

/*
   Assumes:
     logdensity : log of the density kernel of the distribution evaluated at the current point
     metropolis : pointer to valid TMetropolis_block structure

   Results:
     A metropolis draw is stored in metropolis->structure

   Returns:
     The log of the density kernel evaluated at the current point
*/
PRECISION metropolis_draw_all_blocks(PRECISION logdensity, TMetropolis_block *metropolis)
{
  int i, k, n=metropolis->n_parameters;
  TVector block, block_old;
  PRECISION logdensity_old;

  block_old=CreateVector(n);
  block=CreateVector(n);

  logdensity_old=logdensity;                                 
  metropolis->pGetParameters(structure,pElementV(block_old));  

  EquateVector(block,block_old);

  for (k=i=0; i < metropolis->n_blocks; i++)
    {
      for ( ; k < metropolis->block_offsets[i+1]; k++)
        if (metropolis->direction[k])
	  UpdateV(1.0,block,dw_gaussian_rnd()*ElementV(metropolis->scale,k),metropolis->direction[k]);
        else
          ElementV(block,k)+=dw_gaussian_rnd()*ElementV(metropolis->scale,k);
      
      metropolis->pPutParameters(structure,pElementV(block));     
      logdensity=metropolis->pDensity(structure);              

      if (log(dw_uniform_rnd()) <= logdensity - logdensity_old)
        {
          metropolis->jumps[i]++;
	  if (i < metropolis->n_blocks-1) EquateVector(block_old,block);
	  logdensity_old=logdensity;
        }
      else
	if (i < metropolis->n_blocks-1)
	  EquateVector(block,block_old);
	else
	  metropolis->pPutParameters(structure,pElementV(block_old));

      metropolis->draws[i]++;
    }

  FreeVector(block);
  FreeVector(block_old);

  return logdensity_old;
}

/*
   Assumes:
     logdensity : log of the density kernel of the distribution evaluated at the current point
     block      : 0 <= block < metropolis->n_blocks
     metropolis : pointer to valid TMetropolis_block structure

   Results:
     A metropolis draw of block is stored in metropolis->structure

   Returns:
     The log of the density kernel evaluated at the current point
*/
PRECISION metropolis_draw_single_block(PRECISION logdensity, int block, TMetropolis_block *metropolis)
{
  int k, n=metropolis->n_parameters;
  TVector block, block_old;
  PRECISION logdensity_old;

  block_old=CreateVector(n);
  block=CreateVector(n);

  logdensity_old=logdensity;    
  metropolis->pGetParameters(structure,pElementV(block_old));  

  EquateVector(block,block_old);

  for (k=metropolis->block_offsets[block]; k < metropolis->block_offsets[block+1]; k++)
    if (metropolis->direction[k])
      UpdateV(1.0,block,dw_gaussian_rnd()*ElementV(metropolis->scale,k),metropolis->direction[k]);
    else
      ElementV(block,k)+=dw_gaussian_rnd()*ElementV(metropolis->scale,k);
      
  metropolis->pPutParameters(structure,pElementV(block));       
  logdensity=metropolis->pDensity(structure);              

  if (log(dw_uniform_rnd()) <= logdensity - logdensity_old)
    {
      metropolis->jumps[block]++;
      logdensity_old=logdensity;
    }
  else
    metropolis->pPutParameters(structure,pElementV(block_old));

  metropolis->draws[block]++;

  FreeVector(block);
  FreeVector(block_old);

  return logdensity_old;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/****************************** Adaptive Scaling *******************************/
/*******************************************************************************/
TAdaptive* CreateAdaptive(PRECISION mid)
{
  TAdaptive *adaptive=(TAdaptive*)dw_malloc(sizeof(TAdaptive));

  if ((mid <= 0.0) || (mid >= 1.0))
    {
      printf("SetAdaptiveCenter(): center out of range\n");
      dw_exit(0);
    }

  adaptive->mid=mid;
  adaptive->log_mid=log(mid);
  adaptive->lower_bound=exp(adaptive->log_mid/0.2);
  adaptive->upper_bound=exp(adaptive->log_mid/5.0);

  return adaptive;
}

void SetAdaptiveCenter(PRECISION mid, TAdaptive *adaptive)
{
  if ((mid <= 0.0) || (mid >= 1.0))
    {
      printf("SetAdaptiveCenter(): center out of range\n");
      dw_exit(0);
    }

  adaptive->mid=mid;
  adaptive->log_mid=log(mid);
  adaptive->lower_bound=exp(adaptive->log_mid/0.2);
  adaptive->upper_bound=exp(adaptive->log_mid/5.0);
}

void InitializeAdaptive(int period, int block, TAdaptive *adaptive, TMetropolis_block *metropolis)
{
  adaptive->period=period;
  adaptive->end_draws=metropolis->draws[block] + period;
  adaptive->begin_draws=metropolis->draws[block];
  adaptive->begin_jumps=metropolis->jumps[block]=0;
  adaptive->best_scale=adaptive->scale=1.0;
  adaptive->previous_ratio=0.0;
  adaptive->low_scale=adaptive->low_jump_ratio=adaptive->high_scale=adaptive->high_jump_ratio=-1.0;
}

PRECISION RecomputeScale(int end_jumps, int end_draws, TAdaptive *adaptive)
{
  PRECISION new_scale, diff;

  // Compute jump ratio
  adaptive->previous_ratio=(PRECISION)(end_jumps - adaptive->begin_jumps)/(PRECISION)(end_draws - adaptive->begin_draws);

  // Recompute scale?
  if (adaptive->end_draws > end_draws) return adaptive->scale;

  // Set new low or high bounds
  if (adaptive->previous_ratio < adaptive->mid)
    {
      adaptive->low_scale=adaptive->scale;
      adaptive->low_jump_ratio=adaptive->previous_ratio;
    }
  else
    {
      adaptive->high_scale=adaptive->scale;
      adaptive->high_jump_ratio=adaptive->previous_ratio;
    }

  // Compute new scale and best scale
  if (adaptive->low_jump_ratio < 0.0)
    {
      adaptive->best_scale=adaptive->scale;
      new_scale=((adaptive->previous_ratio > adaptive->upper_bound) ? 5.0 
		 : adaptive->log_mid/log(adaptive->previous_ratio))*adaptive->high_scale;
    }
  else 
    if (adaptive->high_jump_ratio < 0.0)
      {
        adaptive->best_scale=adaptive->scale;
        new_scale=((adaptive->previous_ratio < adaptive->lower_bound) ? 0.2 
		   : adaptive->log_mid/log(adaptive->previous_ratio))*adaptive->low_scale;
      }
    else
      {
        new_scale=adaptive->best_scale=((diff=adaptive->high_jump_ratio - adaptive->low_jump_ratio) > 1.0e-6) 
	  ? ((adaptive->mid - adaptive->low_jump_ratio)*adaptive->low_scale 
	     + (adaptive->high_jump_ratio - adaptive->mid)*adaptive->high_scale)/diff 
	  : 0.5*(adaptive->low_scale + adaptive->high_scale);
        adaptive->period*=2;
        adaptive->low_jump_ratio=adaptive->high_jump_ratio=-1.0;
      }

  // Reset adaptive counts and scale
  adaptive->begin_jumps=end_jumps;
  adaptive->begin_draws=end_draws;
  adaptive->end_draws=end_draws + adaptive->period;
  adaptive->scale=new_scale;

  return new_scale;
}

/*
   Prints jumping info:
    block number (zero based)
    total jump ratio for all prevuous sub-runs
    jump ratio for previouus sub-run
    scale for current sub-run
    total number draws for current sub-run
    sub-run length
    
*/
void PrintJumpingInfo(int b, TAdaptive *adaptive, TMetropolis_block *metropolis)
{
  PRECISION total_jump_ratio=(PRECISION)(metropolis->jumps[b])/(PRECISION)(metropolis->draws[b]);
  printf("block: %d  (%lf %lf %lf %d %d)\n",b,total_jump_ratio,adaptive->previous_ratio,adaptive->scale,
    metropolis->draws[b] - adaptive->begin_draws,adaptive->period);
}

void AdaptiveScale_metropolis_block(TMetropolis_block *metropolis, PRECISION mid, int period, int max_period, int verbose)
{
  PRECISION new_scale, old_scale, rescale, logdensity;
  TAdaptive **adaptive;
  int  done=0, i, k, begin_time=(int)time((time_t*)NULL), count=0, check=period;

  adaptive=(TAdaptive**)dw_CreateArray_pointer(metropolis->n_blocks,dw_free);
  for (k=metropolis->n_blocks-1; k >= 0; k--)
    InitializeAdaptive(period,k,adaptive[k]=CreateAdaptive(mid),metropolis);

  if (verbose)
    printf("Beginning adaptive burn in -- maximum period = %d.\n",max_period);

  logdensity=metropolis->pDensity(metropolis->structure);
  while (!done)
    {
      count++;

      logdensity=draw_metropolis(model,logdensity);

      if (count == check)
	{
          done=1;

          if (verbose)
	    printf("\n%d iterations completed - elapsed time: %d seconds\n",count,(int)time((time_t*)NULL) - begin_time);

	  for (k=metropolis->n_blocks-1; k >= 0; k--)
            {
              old_scale=adaptive[k]->scale;
              new_scale=RecomputeScale(metropolis->jumps[k],metropolis->draws[k],adaptive[k]);
              if (new_scale != old_scale)
                for (rescale=new_scale/old_scale, i=metropolis->block_offsets[k]; i < metropolis->block_offsets[k+1]; i++)
                  ElementV(metropolis->scale,i)*=rescale;
              if (adaptive[k]->period <= max_period) done=0;
              if (verbose) PrintJumpingInfo(k,adaptive[k],metropolis);
            }
   
          check+=period;
	}
    }

  for (i=k=0; k < metropolis->n_blocks; k++)
    if (adaptive[k]->scale != adaptive[k]->best_scale)
      for (rescale=adaptive[k]->best_scale/adaptive[k]->scale; i < metropolis->block_offsets[k+1]; i++)
        ElementV(metropolis->scale,i)*=rescale;

  dw_FreeArray(adaptive);
}

void AdaptiveScale_metropolis_block_single_block(int block, TMetropolis_block *metropolis, PRECISION mid, int period, int max_period, int verbose)
{
  TAdaptive *adaptive;
  int  done=0, i, begin_time=(int)time((time_t*)NULL), count=0, check=period;
  PRECISION new_scale, old_scale, rescale, logdensity;

  if ((block < 0) || (block >= metropolis->n_blocks)) 
    {
      if (verbose) printf("AdaptiveScale_metropolis_block_singleblock(): Invalid value for block\n");
      return;
    }

  InitializeAdaptive(period,block,adaptive=CreateAdaptive(mid),metropolis);

  if (verbose)
    printf("Beginning adaptive burn in for block %d -- maximum period = %d.\n",block,max_period);

  logdensity=metropolis->pDensity(metropolis->structure);
  while (!done)
    {
      count++;
      
      if (metropolis->pDrawGibbs) logdensity=metropolis->pDrawGibbs(logdensity,metropolis->structure);
      logdensity=metropolis_draw_single_blocks(logdensity,block,metropolis);

      if (count == check)
        {
          if (verbose)
            printf("\n%d iterations completed - elapsed time: %d seconds\n",count,(int)time((time_t*)NULL) - begin_time);

          old_scale=adaptive->scale;
          new_scale=RecomputeScale(metropolis->jumps[block],metropolis->draws[block],adaptive);
          if (new_scale != old_scale)
            for (rescale=new_scale/old_scale, i=metropolis->block_offsets[block]; i < metropolis->block_offsets[block+1]; i++)
              ElementV(metropolis->scale,i)*=rescale;
          if (adaptive->period > max_period) done=1;
          if (verbose) PrintJumpingInfo(block,adaptive,metropolis);
                                   
          check+=period;
	}
    }

  if (adaptive->scale != adaptive->best_scale)
    for (rescale=adaptive->best_scale/adaptive->scale, i=metropolis->block_offsets[block]; 
	 i < metropolis->block_offsets[block+1]; i++)
      ElementV(metropolis->scale,i)*=rescale;

  dw_free(adaptive);
}

/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/

/*
   Assumes
    metropolis : pointer to valid TMetropolis_block structure
    center     : target value for acceptance ration
    peroid     : initial length of sub-runs to calibrate scales
    max_period : maximum length of sub-runs to calibrate scales
    verbose    : non-zero for output to stdout

   Results
    The values of metropolis->scale are calibrated to obtain the desired 
    acceptance ratio. 

   Notes:
    The calling routine should save the initial parameters before entry and reset
    them upon return.
*/
void Calibrate_metropolis_block(TStateModel *model, PRECISION center, int period, int max_period, int verbose)
{
  /* int n_block=NumberFreeParametersTheta(model), n_q=NumberFreeParametersQ(model); */
  /* TVector initial_block=(TVector)NULL, initial_q=(TVector)NULL;  */

  /* // Save initial parameters */
  /* if (n_block) ConvertThetaToFreeParameters(model,pElementV(initial_block=CreateVector(n_block))); */
  /* if (n_q) ConvertQToFreeParameters(model,pElementV(initial_q=CreateVector(n_q))); */

  // Calibrate
  AdaptiveScale_metropolis_block(metropolis,center,period,max_period,verbose);

  /* // Reset initial parameters */
  /* if (n_block)  */
  /*   { */
  /*     ConvertFreeParametersToBlock(model,pElementV(initial_block)); */
  /*     FreeVector(initial_block); */
  /*   } */
  /* if (n_q)  */
  /*   { */
  /*     ConvertFreeParametersToQ(model,pElementV(initial_q)); */
  /*     FreeVector(initial_q); */
  /*   } */
}

/*
   Assumes
    idx        : 0 <= idx < metropolis->n_parameters
    metropolis : pointer to valid TMetropolis_block structure
    center     : target acceptance rate
    peroid     : initial length of sub-runs to calibrate scale for direction idx
    max_period : maximum length of sub-runs to calibrate scale for direction idx
    verbose    : non-zero for output to stdout

   Returns
    The total number of draws made.  A single draw consists of a draw of the 
    states, a draw of the transition matrix parameters, and a draw of block.  The
    draw of block is only in the direction idx.

   Results
    The value of ElementV(model->metropolis_scale_block,idx) is calibrated to 
    obtain the desired acceptance ratio.

   Notes:
    The calling routine should save the initial parameters before entry and reset
    them upon return.
*/
int Calibrate_metropolis_block_single_direction(int idx, TMetropolis_block *metropolis, PRECISION center, int period, int max_period, int verbose)
{
  int *dims, n_draws, n_blocks;

  // Save old blocks
  n_blocks=metropolis->n_blocks;
  dims=(int*)dw_malloc(n_blocks*sizeof(int));
  memcpy(dims,metropolis->block_dims,n_blocks*sizeof(int));

  // Setup blocks 
  Setup_metropolis_block_blocks_full(metropolis);

  // Calibrate
  AdaptiveScale_metropolis_block_single_block(idx,model,center,period,max_period,verbose);
  n_draws=metropolis->draws[idx];

  // Reset blocks
  Setup_metropolis_block_blocks(metropolis,n_blocks,dims);
  dw_free(dims);

  return n_draws;
}

/*
   Assumes
    model        : pointer to valid TStateModel structure
    center_s     : target acceptance rate for each direction invidually
    peroid_s     : initial length of sub-runs to calibrate scale for each direction individually
    max_period_s : maximum length of sub-runs to calibrate scale for each direction individually
    center_a     : target acceptance rate for each block
    peroid_a     : initial length of sub-runs to calibrate scales for each block
    max_period_a : maximum length of sub-runs to calibrate scales for each block
    verbose      : non-zero for output to stdout

   Returns
    The total number of block draws made.

   Results
    The values of model->metropolis_scale_block are calibrated to obtain the 
    desired acceptance ratio.
*/
int Calibrate_metropolis_block_two_pass(TStateModel *model, PRECISION center_s,	int period_s, int max_period_s, 
					PRECISION center_a, int period_a, int max_period_a, int verbose)
{
  TMetropolis_block *metropolis=model->metropolis_block;
  int *dims, n_draws=0, n_blocks, i, n_theta=NumberFreeParametersTheta(model), n_q=NumberFreeParametersQ(model);
  TVector initial_theta=(TVector)NULL, initial_q=(TVector)NULL; 

  // save initial value
  if (n_theta) ConvertBlockToFreeParameters(model,pElementV(initial_theta=CreateVector(n_theta)));
  if (n_q) ConvertQToFreeParameters(model,pElementV(initial_q=CreateVector(n_q)));

  // Individual direction run
  if (period_s > 0)
    {
      // Save old blocks
      n_blocks=metropolis->n_blocks;
      memcpy(dims=(int*)dw_malloc(n_blocks*sizeof(int)),metropolis->block_dims,n_blocks*sizeof(int));

      // Setup blocks 
      Setup_metropolis_block_blocks_full(metropolis);

      // Calibrate
      for (i=0; i < n_block; i++)
        {
          AdaptiveScale_metropolis_block_single_block(i,model,center_s,period_s,max_period_s,verbose);
          n_draws+=metropolis->draws[i];
          if (n_theta) ConvertFreeParametersToBlock(model,pElementV(initial_theta));
          if (n_q) ConvertFreeParametersToQ(model,pElementV(initial_q));
        }
    
      // Reset blocks
      Setup_metropolis_block_blocks(metropolis,n_blocks,dims);
      dw_free(dims);
    }
  else
    ResetCounts_metropolis_block(metropolis);

  // Final run
  if (period_a > 0)
    {
      AdaptiveScale_metropolis_block(model,center_a,period_a,max_period_a,verbose);
      for (i=0; i < metropolis->n_blocks; i++) n_draws+=metropolis->draws[i];
      if (n_theta) ConvertFreeParametersToBlock(model,pElementV(initial_theta));
      if (n_q) ConvertFreeParametersToQ(model,pElementV(initial_q));
    }

  if (n_theta) FreeVector(initial_theta);
  if (n_q) FreeVector(initial_q);

  return n_draws;
}
