#include <stdio.h>
#include <R.h>
#include <Rmath.h>
#include <math.h>
#include <Rinternals.h>

/* Define variables rr and qq for adapting proposals during pilot tuning */
#define rr 1.0
#define qq 1.25
#define newgs 10.0

/* 
Define function MCMCloop to draw samples from the posterior distribution

  Parameters that are returned:
    eta_z = Correlation between consecutive movement directions in centre of attraction state z
    m_z = Intercept term for strength of bias to centre of attraction z
    r_z = Strength of bias as a function of distance to centre of attraction z
    q_z = Strength of bias as a function of distance^2 to centre of attraction z
    X_z = Location of the X coordinate for centre of attraction z
    Y_z = Location of the Y coordinate for centre of attraction z
    a = Scale parameter of the Weibull distribution for step length (for centres of attraction, when delta_t > d_z)  
    b = Shape parameter of the Weibull distriubtion for step length (for centres of attraction, when delta_t > d_z)    
    a2 = Scale parameter of the Weibull distribution for step length (when delta_t < d_z)  
    b2 = Shape parameter of the Weibull distriubtion for step length (when delta_t < d_z)  
    d_z = Change-point distance of Weibull distribution parameters for centre of attraction states
    upsilon = Correlation between consecutive movement directions in an exploratory state
    phi_0 = Initial movement direction
    tau = Variance of the Normal(0,tau) prior distributions for r_z and q_z
    sigma2_x = Variance of the N(0,sigma2_x) observation error term in the x direction
    sigma2_y = Variance of the N(0,sigma2_y) observation error term in the y direction
    X_t = Predicted location of the X coordinate at time t
    Y_t = Predicted location of the Y coordinate at time t
    psi = State transition probabilities
    z_t = State assignment for each time step t
    centre_loc = Vector indicating which predicted locations (X_t, Y_t) are the current centres of attraction
    Model = Vector indicating the model structure; '1' indicates a (state-specific) parameter currently in the model from the full set ordered by eta_z, q_z, m_z, and upsilon (z = 1,...,c).  For example, for c=3, 0111000101 indicates eta_2, eta_3, q_1, m_2, and upsilon are currently in the model 
    sigma_scale = Scale parameter for the inverse gamma prior on sigma2_x and sigma2_y
    sigma_shape = Shape parameter for the inverse gamma prior on sigma2_x and sigma2_y
    tau_scale = Scale parameter for the inverse gamma prior on tau
    tau_shape = Shape parameter for the inverse gamma prior on tau
    bupper = Upper limit for Unif(0,bupper) prior on Weibull shape parameter (b)

  Other variables returned:
    loglike = Value of the likelihood function
    gs = Vector of values for (adaptive) proposal distributions
    arate = Metropolis-Hasting acceptance rate
    
  Data:
    x_ti = Observed location of the X coordinate at time t, i (includes missing locations where i=0) 
    y_ti = Observed location of the Y coordinate at time t, i (includes missing locations where i=0)
    N = Number of predicted locations (T+1)
    fparms = Number of parameters, excluding z_t, psi, (X_t, Y_t), and missing (x_ti, y_ti)
    numbercentres = Number of potential centres of attraction (c)
    numberexplore = Number of potential exploratory states (h)
    ssidx = Vector for looping over the observed location (x_ti, y_ti) within each time step t
    j_ti = Vector indicating the proportion of each interval (t-1,t) at which the ith observation (x_ti,y_ti) was obtained.  For any interval where i=0, j_ti = -1.
    nvt = Number of vertices for the polygon evaluated in PNPOLY() function
    nogo1x = X coordinates of vertices for polygon indicating inland areas
    nogo1y = Y coordinates of vertices for polygon indicating inland areas   
    nogo2x = X coordinates of vertices for polygon indicating inland areas
    nogo2y = Y coordinates of vertices for polygon indicating inland areas 
    nogo3x = X coordinates of vertices for polygon indicating inland areas
    nogo3y = Y coordinates of vertices for polygon indicating inland areas   
    nogo4x = X coordinates of vertices for polygon indicating inland areas
    nogo4y = Y coordinates of vertices for polygon indicating inland areas 
    maxdist = Maximum distance between observed locations; used to scale distances within GETRHO() function
    maxstep = Maximum step length
    
  Markov chain specifications:
    iter = Number of iterations of the Markov chain
    thin = Number of iterations for thinning of the Markov chain
    adapt = Number of iterations for pilot tuning
    bin = Number of iterations between each pilot tune
    taccept = Target acceptance rate for Metropolis-Hastings updates
    txyaccept = Target acceptance rate for (X_t,Y_t) MH block updates
    tcentreaccept =  Target acceptance rate for (X_z,Y_z) MH updates
    print_iteration_count = Indicator for whether to print iteration count (=1) or not (=0)

*/

void MCMCloop(double *eta_z, double *m_z, double *r_z, double *q_z, double *X_z, double *Y_z, double *a, double *b, double *a2, double *b2, double *d_z, double *upsilon, double *phi_0, double *tau, double *sigma2_x, double *sigma2_y,  
          double *X_t, double *Y_t, double *psi, int *z_t, int *centre_loc, int *Model,
          double *sigma_scale, double *sigma_shape, double *tau_scale, double *tau_shape, double *b_upper,
          double *loglike, double *gs, double *arate, 
          double *x_ti, double *y_ti, int *N, int *fparms, int *numbercentres, int *numberexplore, int *ssidx, double *j_ti, 
          int *nvt, double *nogo1x, double *nogo1y, double *nogo2x, double *nogo2y, double *nogo3x, double *nogo3y, double *nogo4x, double *nogo4y, double *maxdist, double *maxstep,  
          int *iter, int *thin, int *adapt, int *bin, double *taccept, double *txyaccept, double *tcentreaccept, int *print_iteration_count)
{

  /* Declare functions */
  double LIKE();          /* Calculates joint likelihood */
  double LIKEXY();        /* Calculates conditional likelihood for updating each position (X_t, Y_t) */
  void GETZ();            /* Updates state assigments (z_t) for each time step */
  double ZPRIOR();        /* Calculates prior density for z_ts */
  void GETPSI();          /* Updates state transition probabilities (psi) */  
  double DINVGAMMA();     /* Inverse Gamma probability density function */
  int PNPOLY();           /* Tests whether locations lie within n-sided polygon) */
  int PROPCENTRE();       /* Proposes new position for centres of attraction from predicted locations (X_t, Y_t) */
  int GETMk();            /* Calculate number of locations (X_t, Y_t) within some distance of centre of attraction (X_z, Y_z) */
  double GETSTEP();       /* Calculate step length between two locations */
  
  /* Declare and initialize variables */
  int niter, th, ada, n;
  
  niter = *iter;              /* Number of iterations in the Markov chain */
  th = *thin;                 /* Number of iterations for thinning */
  ada = *adapt;               /* Number of iterations for pilot tuning */
  n = *bin;                   /* Number of iterations between each pilot tune */
  
  int nsteps, firstparms, ncentres, nexplore;   
  nsteps = *N;                      /* Number of predicted locations (T+1) */
  firstparms = *fparms;             /* Number of parameters, excluding z_t, psi, (X_t, Y_t), and missing (x_ti, y_ti) */
  ncentres = *numbercentres;        /* Number of potential centres of attraction */
  nexplore = *numberexplore;        /* Number of potential exploratory states */
  int states = ncentres+nexplore;   /* Total number of potential states */

  double sigmascale, sigmashape, tauscale, taushape, bupper, mdist, mstep, targetaccept, targetxyaccept, targetcentreaccept; 
  sigmascale = *sigma_scale;      /* Scale parameter for inverse gamma prior on sigma2_x and sigma2_y */
  sigmashape = *sigma_shape;      /* Shape parameter for inverse gamma prior on sigma2_x and sigma2_y */
  tauscale = *tau_scale;          /* Scale parameter for inverse gamma prior on tau */
  taushape = *tau_shape;          /* Shape parameter for inverse gamma prior on tau */
  bupper = *b_upper;              /* Upper limit for Unif(0,bupper) prior on Weibull shape parameter (b) */
  mdist = *maxdist;               /* Maximum distance between observed locations; used to scale distances within GETRHO() function   */
  mstep = *maxstep;               /* Maximum possible step length */
  targetaccept = *taccept;        /* Target acceptance rate for Metropolis-Hastings updates */
  targetxyaccept = *txyaccept;        /* Target acceptance rate for (X_t,Y_t) MH updates */
  targetcentreaccept = *tcentreaccept;   /* Target acceptance rate for (X_z,Y_z) MH updates */
          
  double ll;                            /* Current value for log-likelihood */
  double nl, ol;                        /* Proposed and current likelihood for use in Metropolis Hastings ratio */
  double nprior, oprior, nprop, oprop;  /* Proposed and current prior/proposal densities for use in MH ratio */ 
  double npropz, opropz;                /* Proposed and current prior/proposal densities for z_ts */
    
  int nvert;                            /* Number of vertices for the polygons evaluated in PNPOLY() function */
  nvert = *nvt;
    
  /* Declare variables to store current (s) values of parameters */  
  double eta_zs[ncentres], m_zs[ncentres], r_zs[ncentres], q_zs[ncentres], upsilons[nexplore];
  double X_zs[ncentres], Y_zs[ncentres], X_ts[nsteps], Y_ts[nsteps]; 
  double as[states], a2s[ncentres], bs[states], b2s[ncentres], d_zs[ncentres], phi_0s, taus, sigma2_xs, sigma2_ys;

  /* Declare variables for proposed (star) values of parameters */
  double eta_zstar[ncentres], m_zstar[ncentres], r_zstar[ncentres], q_zstar[ncentres], upsilonstar[nexplore];
  double X_zstar[ncentres], Y_zstar[ncentres], X_tstar[nsteps], Y_tstar[nsteps];
  double astar[states], a2star[ncentres], bstar[states], b2star[ncentres], d_zstar[ncentres], phi_0star, taustar, sigma2_xstar, sigma2_ystar; 

  double psis[(states)*(states)]; /* Vector storing current values of state transition probabilities (psi) */
  double pvect[states];               /* Row vector of state transition probabilities from state z, psi[z,.], returned by function GETPSI()   */
                            
  int z_ts[nsteps];                       /* Vector of current state assignments for time steps 0,...,T */
  int z_tstar[nsteps];                    /* Vector of proposed state assignments for time steps 0,...,T */
  double zdens[nsteps];                   /* Vector for storing conditional density of state assignments for time steps 0,...,T */
  int centre_locs[ncentres];              /* Vector indicating which predicted locations (X_t, Y_t) are the current centres of attraction */
  int centre_locstar[ncentres];           /* Vector indicating which predicted locations (X_t, Y_t) are the proposed centres of attraction */         
  double centredist;                      /* Variable indicating the distance from predicted location to centre of attraction z */
  
  /* Miscellaneous variables */
  int i, g, j, jj;                   /* For loops */
  int parm1, parm2;                       /* For specifying parameter numbers for (X_t, Y_t) updates */
  double phi_0temp;                       /* Temporary proposal phi_0  */
  double w_n=0.0;                         /* Denominator for calculating acceptance rate */
  double invgs, sha, sca;                 /* Random walk proposal parameters */
  int Mk[ncentres], Mkstar[ncentres];     /* Number of current (or proposed) locations within some distance of current (or proposed) centre of attraction */ 
  int centre_ind=0;                       /* Centre indicator for when location (X_t, Y_t) is a centre of attraction (centre_ind=1,...,ncentres) or not (centre_ind=0) */
  
  /* Initial parameter values */
  for (j=0; j < ncentres; j++)  {
    eta_zs[j]=eta_z[j];
    m_zs[j]=m_z[j];
    r_zs[j]=r_z[j];
    q_zs[j]=q_z[j];
    X_zs[j]=X_z[j];
    Y_zs[j]=Y_z[j];
    a2s[j]=a2[j];
    b2s[j]=b2[j];
    d_zs[j]=d_z[j];
    centre_locs[j]=centre_loc[j];
  }
  
  for (j=0; j < (states); j++)  {     
    as[j]=a[j];
    bs[j]=b[j];
  }

  for (j=0; j<nexplore; j++)  {
    upsilons[j] = upsilon[j];
  }
  
  phi_0s = phi_0[0];
  taus = tau[0];
  sigma2_xs = sigma2_x[0];
  sigma2_ys = sigma2_y[0];
          
  for (j=0; j < nsteps; j++)  {
    z_ts[j]=z_t[j];
    z_tstar[j]=z_t[j];
    zdens[j]=0;
    X_ts[j] = X_t[j];
    Y_ts[j] = Y_t[j];
    X_tstar[j] = X_ts[j];
    Y_tstar[j] = Y_ts[j];
  } 
  
  for (j=0; j < (states)*(states); j++)  {
    psis[j]=psi[j];
  }
  
  for(j=0; j<ncentres; j++) {
    centredist = 1/gs[4*ncentres+j];
    Mk[j] = GETMk(j, nsteps, X_zs, Y_zs, X_ts, Y_ts, centredist);
  }

  /* Define variables to store current model structure for eta_z, r_z, q_z, and upsilon */  
  int Model_eta[ncentres], Model_q[ncentres], Model_upsilon[nexplore];
  
  /* Define variables to store proposed model structure for eta_z, r_z, q_z, and upsilon */ 
  int Model_etastar[ncentres], Model_qstar[ncentres], Model_upsilonstar[nexplore];

  /* Intial model for eta_z, r_z, q_z, and upsilon */  
  for (j=0; j < ncentres; j++)  {
    Model_eta[j]=Model[j];
    Model_q[j]=Model[j+ncentres];
  }
  for (j=0; j<nexplore; j++)  {
    Model_upsilon[j]=Model[2*ncentres+j];
  }

  /* Vector to store numerator of acceptance rate for each parameter (except z_t and psi, which use Gibbs sampler) */   
  double ap[firstparms+2*nsteps];       
  for (j=0; j<(firstparms+2*nsteps); j++) {
    ap[j]=0.0;
  }
    
  /* Calculate the log-likelihood */  
  loglike[0]=LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
  ll=loglike[0];
 
  int count = 1;    /* Initialize count for thinning */

  /* Begin Markov chain */  
  for (g=0; g < niter; g++)  {
  
  /* Update parameters and latent variables in the current model */
  
    /* Block update predicted locations (X_t, Y_t) using normal random walk MH step */
    for (i=0; i < nsteps; i++)  {

      parm1 = firstparms+i;
      invgs=1/gs[parm1];
      X_tstar[i]=X_ts[i]+rnorm(0.0,invgs);
      
      parm2 = firstparms+nsteps+i;
      invgs=1/gs[parm2];
      Y_tstar[i]=Y_ts[i]+rnorm(0.0,invgs);
          
      /* Check that proposed (X_tstar, Y_tstar) are not located within prohibited areas. If so, reject */
      if(!(PNPOLY(nvert, nogo1x, nogo1y, X_tstar[i], Y_tstar[i]) + PNPOLY(nvert, nogo2x, nogo2y, X_tstar[i], Y_tstar[i]) + PNPOLY(nvert, nogo3x, nogo3y, X_tstar[i], Y_tstar[i]) + PNPOLY(nvert, nogo4x, nogo4y, X_tstar[i], Y_tstar[i]))) {

        centre_ind=0;
        oprop=0.0;
        nprop=0.0; 
        for (jj=0; jj<ncentres; jj++)  {
          if(i==centre_locs[jj]) {
            centre_ind = jj+1;
          } else  {
            /* Ensure that proposed locations remain within specified proposal radius of centres of attraction (for proposing new centre_locs for centres of attractions) */
            centredist = 1/gs[4*ncentres+jj];
            nprop += log(1.0/Mk[jj]);
            if(!isfinite(nprop)) {nprop = HUGE_VAL; Rprintf("error: no locations within specified proposal radius (%f metres) of centre of attraction %d \n",centredist,jj+1);} 
            Mkstar[jj]=Mk[jj]-((GETSTEP(X_ts[i], Y_ts[i], X_zs[jj], Y_zs[jj])<centredist)-(GETSTEP(X_tstar[i], Y_tstar[i], X_zs[jj], Y_zs[jj])<centredist));
            oprop += log(1.0/(Mkstar[jj]));
            if(!isfinite(oprop)) {oprop = -HUGE_VAL; Rprintf("warning: no proposed locations within specified proposal radius (%f metres) of centre of attraction %d \n",centredist,jj+1);}
          }
          X_zstar[jj]=X_zs[jj];
          Y_zstar[jj]=Y_zs[jj];
        }

        /* Calculate conditional density for (X_tstar, Y_tstar) */        
        nl = LIKEXY(i, 0, eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_tstar, Y_tstar, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
        ol = LIKEXY(i, 0, eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
 
         /* If (X_t, Y_t) is current location of a centre of attraction, then also propose new (X_z, Y_z) = (X_t, Y_t) */
        if(centre_ind) {
          X_zstar[centre_ind-1]=X_tstar[i];
          Y_zstar[centre_ind-1]=Y_tstar[i];
          centredist = 1/gs[4*ncentres+centre_ind-1]; 
          nprop += log(1.0/Mk[centre_ind-1]); 
          if(!isfinite(nprop)) {nprop = HUGE_VAL; Rprintf("error: no locations within specified proposal radius (%f metres) of centre of attraction %d \n",centredist,centre_ind);} 
          Mkstar[centre_ind-1]=GETMk(centre_ind-1, nsteps, X_zstar, Y_zstar, X_tstar, Y_tstar, centredist);
          oprop += log(1.0/Mkstar[centre_ind-1]);
          if(!isfinite(oprop)) {oprop = -HUGE_VAL; Rprintf("warning: no locations within specified proposal radius (%f metres) of proposed centre of attraction %d \n",centredist,centre_ind);} 

          /* Calculate full likelihood if (X_t, Y_t) is a current centre of attraction */  
          nl=LIKE(eta_zs, m_zs, r_zs, q_zs, X_zstar, Y_zstar, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_tstar, Y_tstar, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
          ol=LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
        }       
    
        if(runif(0.0,1.0)<exp(nl+oprop-ol-nprop)) {  /* Evaluate MH ratio. Uniform priors and normal proposals cancel out */
          X_ts[i]=X_tstar[i];
          parm1 = firstparms+i;
          ap[parm1] += pow(rr,(n-1 - g % (n)));
          
          Y_ts[i]=Y_tstar[i];
          parm2 = firstparms+nsteps+i;
          ap[parm2] += pow(rr,(n-1 - g % (n)));
            
          if(centre_ind) {
            X_zs[centre_ind-1]=X_zstar[centre_ind-1];
            Y_zs[centre_ind-1]=Y_zstar[centre_ind-1];
          }
          for (jj=0; jj<ncentres; jj++)  {
            Mk[jj]=Mkstar[jj];
          }
        } else  {  
          X_tstar[i]=X_ts[i];
          Y_tstar[i]=Y_ts[i];
        }
      } else  {  
          X_tstar[i]=X_ts[i];
          Y_tstar[i]=Y_ts[i];
      }
    }

    /* Calculate the current log-likelihood */  
    ll=LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);      
 
    /* Update observation error terms using gamma random walk MH step  */
      /* Update sigma2_x */
      sha=gs[firstparms-2];
      sca=sigma2_xs/sha;
      sigma2_xstar=rgamma(sha,sca);
      nl= LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xstar, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
      nprior=log(DINVGAMMA(sigma2_xstar,sigmashape,sigmascale));  /* Inverse gamma prior */
      oprior=log(DINVGAMMA(sigma2_xs,sigmashape,sigmascale));     /* Inverse gamma prior */      
      nprop=dgamma(sigma2_xstar,sha,sca,1); /* Gamma random walk proposal */
      sca=sigma2_xstar/sha;
      oprop=dgamma(sigma2_xs,sha,sca,1);    /* Gamma random walk proposal */
      if(runif(0.0,1.0)<exp(nl+nprior+oprop-ll-oprior-nprop)) {   /* Evaluate MH ratio */
        sigma2_xs=sigma2_xstar;
        ll=nl;
        ap[firstparms-2] += pow(rr,(n-1 - g % (n)));
      }
      
      /* Update sigma2_y */    
      sha=gs[firstparms-1];
      sca=sigma2_ys/sha;
      sigma2_ystar=rgamma(sha,sca);     
      nl= LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ystar, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
      nprior=log(DINVGAMMA(sigma2_ystar,sigmashape,sigmascale));  /* Inverse gamma prior */
      oprior=log(DINVGAMMA(sigma2_ys,sigmashape,sigmascale));     /* Inverse gamma prior */     
      nprop=dgamma(sigma2_ystar,sha,sca,1); /* Gamma random walk proposal */
      sca=sigma2_ystar/sha;
      oprop=dgamma(sigma2_ys,sha,sca,1);    /* Gamma random walk proposal */
      if(runif(0.0,1.0)<exp(nl+nprior+oprop-ll-oprior-nprop)) {   /* Evaluate MH ratio */
        sigma2_ys=sigma2_ystar;
        ll=nl;
        ap[firstparms-1] += pow(rr,(n-1 - g % (n)));
      }
    
    /* Update eta_z using random walk MH step */
    for(j=0; j<ncentres; j++)  {
      
      /* Only update if eta_j is in current model */
      if(Model_eta[j]==1)  {

        invgs=1/gs[j];

        for(jj=0; jj<ncentres; jj++)  {
          eta_zstar[jj]=eta_zs[jj];
        }

        eta_zstar[j]=eta_zs[j]+runif((-invgs),invgs);

        if(dunif(eta_zstar[j],0,1,0)>0) {  /* Check that 0 < eta_zstar < 1.  Otherwise, reject */
          nl= LIKE(eta_zstar, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
          if(runif(0.0,1.0)<exp(nl-ll)) {  /* Evaluate MH ratio.  Uniform priors and proposals cancel out */
            ll=nl;
            eta_zs[j]=eta_zstar[j];
            ap[j] += pow(rr,(n-1 - g % (n)));
          }
        }
      }
    }
        
    /* Update m_z, r_z, & q_z using random walk MH step */
    for(j=0; j<ncentres; j++)  {
    
      /* If q_z is not in the current model, update r_z only */
      if(Model_q[j]==0) {

        for(jj=0; jj<ncentres; jj++)  {
          m_zstar[jj]=m_zs[jj];
          r_zstar[jj]=r_zs[jj];
        }
        
        invgs=1/gs[j+ncentres];
        m_zstar[j]=m_zs[j]+runif((-invgs),invgs);
        
        invgs=1/gs[j+2*ncentres];        
        r_zstar[j]=r_zs[j]+runif((-invgs),invgs);
        
        nl= LIKE(eta_zs, m_zstar, r_zstar, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
        nprior=dnorm(m_zstar[j],0,sqrt(taus),1)+dnorm(r_zstar[j],0,sqrt(taus),1);
        oprior=dnorm(m_zs[j],0,sqrt(taus),1)+dnorm(r_zs[j],0,sqrt(taus),1);
                
        if(runif(0.0,1.0)<exp(nl+nprior-ll-oprior)) {   /* Evaluate MH ratio.  Uniform proposals cancel out */
          ll=nl;
          m_zs[j]=m_zstar[j];
          ap[j+ncentres] += pow(rr,(n-1 - g % (n)));
          r_zs[j]=r_zstar[j];
          ap[j+2*ncentres] += pow(rr,(n-1 - g % (n)));
        }
      } else {            /* If q_z is in the current model, block update both r_z and q_z */

        for(jj=0; jj<ncentres; jj++)  {
          m_zstar[jj]=m_zs[jj];
          r_zstar[jj] = r_zs[jj];
          q_zstar[jj] = q_zs[jj];
        }
        
        invgs=1/gs[j+ncentres];
        m_zstar[j]=m_zs[j]+runif((-invgs),invgs);
        
        invgs = 1/gs[j+2*ncentres];
        r_zstar[j] = r_zs[j]+runif((-invgs),invgs);

        invgs = 1/gs[j+3*ncentres];
        q_zstar[j] = q_zs[j]+runif((-invgs),invgs);

        nl = LIKE(eta_zs, m_zstar, r_zstar, q_zstar, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
        nprior=dnorm(m_zstar[j],0,sqrt(taus),1) + dnorm(r_zstar[j],0,sqrt(taus),1) + dnorm(q_zstar[j],0,sqrt(taus),1);
        oprior=dnorm(m_zs[j],0,sqrt(taus),1) + dnorm(r_zs[j],0,sqrt(taus),1) + dnorm(q_zs[j],0,sqrt(taus),1);
        
        if(runif(0.0,1.0)<exp(nl+nprior-ll-oprior)) {     /* Evaluate MH ratio.  Uniform proposals cancel out */
          ll = nl;
          m_zs[j]=m_zstar[j];
          ap[j+ncentres] += pow(rr,(n-1 - g % (n)));
          r_zs[j] = r_zstar[j];
          ap[j+2*ncentres] += pow(rr,(n-1 - g % (n)));
          q_zs[j]=q_zstar[j];
          ap[j+3*ncentres] += pow(rr,(n-1 - g % (n)));        }
      }
    }
    
    /* Update tau using gamma random walk MH step */
    sha = gs[firstparms-3];
    sca = taus/sha;
    taustar = rgamma(sha,sca);
        
    /* Calculate new and old likelihoods.  Note that wCauchy, Weibull, and observation error likelihoods cancel out, hence they are not calculated */
    nl = 0.0;        
    ol = 0.0;
        
    for(j=0; j<ncentres; j++)  {
      nl += dnorm(m_zs[j],0.0,sqrt(taustar),1)+dnorm(r_zs[j],0.0,sqrt(taustar),1);
      ol += dnorm(m_zs[j],0.0,sqrt(taus),1)+dnorm(r_zs[j],0.0,sqrt(taus),1);
      if(Model_q[j]==1)  {
        nl += dnorm(q_zs[j],0.0,sqrt(taustar),1);
        ol += dnorm(q_zs[j],0.0,sqrt(taus),1);
      }
    }
        
    nprior = log(DINVGAMMA(taustar,taushape,tauscale));
    oprior = log(DINVGAMMA(taus,taushape,tauscale));
        
    nprop = dgamma(taustar,sha,sca,1);
    sca = taustar/sha;
    oprop = dgamma(taus,sha,sca,1); 

    if(runif(0.0,1.0)<exp(nl+nprior+oprop-ol-oprior-nprop)) {       /* Evaluate MH ratio */
      taus = taustar;
      ap[firstparms-3] += pow(rr,(n-1 - g % (n)));
    }

    /* Update latent states using Gibbs step   */
    GETZ(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, X_ts, Y_ts, z_ts, centre_locs, psis, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, states, mdist, zdens);
    opropz=0.0;
    for(jj=0; jj<nsteps; jj++)  {
      opropz += zdens[jj];
    }
     
    /* Calculate updated likelihood */    
    ll = LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);

    /* Update transition probabilities using Gibbs step */
    for(j=0; j < states; j++) {
      GETPSI(pvect, z_ts, nsteps, (j+1), states);
      for(jj=0; jj < states; jj++) {
        psis[j*states+jj] = pvect[jj];
      }
    }
    
    /* Update centres of attraction (X_z, Y_z) using discrete uniform prior distribution */
    for (j=0; j < ncentres; j++)  {
    
      for(jj=0; jj<nsteps; jj++)  {
        z_tstar[jj]=z_ts[jj];
      }
      
      for(jj=0; jj<ncentres; jj++)  {
          X_zstar[jj]=X_zs[jj];
          Y_zstar[jj]=Y_zs[jj];
          centre_locstar[jj]=centre_locs[jj];
      }
    
      /* Propose new coordinate for (X_z, Y_z) from corresponding (X_t, Y_t) locations in state z within radius centredist (centredist is constrained to be >= mstep) */
      centredist = 1/gs[4*ncentres+j];
      centre_locstar[j] = PROPCENTRE(j, z_ts, nsteps, X_zs, Y_zs, X_ts, Y_ts, centredist, centre_locs[j]);
      X_zstar[j] = X_ts[centre_locstar[j]]; 
      Y_zstar[j] = Y_ts[centre_locstar[j]];
      
      /* Propose new state assingnments (z_tstar) for proposed centre of attraction location */
      GETZ(eta_zs, m_zs, r_zs, q_zs, X_zstar, Y_zstar, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, X_ts, Y_ts, z_tstar, centre_locstar, psis, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, states, mdist, zdens);
      npropz=0.0;
      for(jj=0; jj<nsteps; jj++)  {
        npropz += zdens[jj];
      }  
      
      nl = LIKE(eta_zs, m_zs, r_zs, q_zs, X_zstar, Y_zstar, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_tstar, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);

      /* Calculate proposal density */      
      nprop = npropz;    
      oprop = opropz; 
      
      /* Check that proposed centre of attraction remains within specified radius (centredist) of other potential locations */
      for(jj=0; jj<ncentres; jj++) {
        centredist = 1/gs[4*ncentres+jj];
        nprop += log(1.0/Mk[jj]); 
        if(!isfinite(nprop)) {nprop = HUGE_VAL; Rprintf("error: no locations within specified proposal radius (%f metres) of centre of attraction %d \n",centredist,jj+1);} 
        Mkstar[jj]=GETMk(jj, nsteps, X_zstar, Y_zstar, X_ts, Y_ts, centredist);
        oprop += log(1.0/Mkstar[jj]);
        if(!isfinite(oprop)) {oprop = -HUGE_VAL; Rprintf("warning: no locations within specified proposal radius (%f metres) of proposed centre of attraction %d \n",centredist,jj+1);} 
      }
      
      /* Calculate prior density */           
      nprior=ZPRIOR(psis,z_tstar,states,nsteps);
      oprior=ZPRIOR(psis,z_ts,states,nsteps);
            
      if(runif(0.0,1.0)<exp(nl+nprior+oprop-ll-oprior-nprop)) {   /* Evaluate MH ratio.  Uniform priors and proposals cancel out */
        ll=nl;
        opropz = npropz;
        X_zs[j]=X_zstar[j];
        ap[ncentres*4+j] += pow(rr,(n-1 - g % (n)));
        gs[firstparms+centre_locstar[j]]=gs[firstparms+centre_locs[j]];
        Y_zs[j]=Y_zstar[j];
        ap[ncentres*5+j] += pow(rr,(n-1 - g % (n)));
        gs[firstparms+nsteps+centre_locstar[j]]=gs[firstparms+nsteps+centre_locs[j]];
        centre_locs[j]=centre_locstar[j];
        for(jj=0; jj<nsteps; jj++)  {
          z_ts[jj]=z_tstar[jj];
        }
        for(jj=0; jj<ncentres; jj++) {
          Mk[jj]=Mkstar[jj];
        }
      }
    }
    
    /* Block update Weibull scale (a), shape (b), and distance change-point (d) parameters using gamma random walk MH step */
      /* Update Weibull parameters for centre of attraction states */
      for(j=0; j < ncentres; j++)  {
        for(jj=0; jj<(states);  jj++) {  
          astar[jj]=as[jj];
          bstar[jj]=bs[jj];
        }
        for(jj=0; jj<(ncentres);  jj++) {  
          a2star[jj]=a2s[jj];
          b2star[jj]=b2s[jj];
          d_zstar[jj]=d_zs[jj];
        }
        sha=gs[ncentres*6+j];
        sca=as[j]/sha;
        astar[j]=rgamma(sha,sca);
        nprop=dgamma(astar[j],sha,sca,1);     /* Gamma proposal */
        sca=astar[j]/sha;
        oprop=dgamma(as[j],sha,sca,1);        /* Gamma proposal */
        nprior=dunif(astar[j],0.0,mstep,1);
        oprior=dunif(as[j],0.0,mstep,1);
        
        sha=gs[ncentres*6+states+j];
        sca=a2s[j]/sha;
        a2star[j]=rgamma(sha,sca);
        nprop+=dgamma(a2star[j],sha,sca,1);     /* Gamma proposal */
        sca=a2star[j]/sha;
        oprop+=dgamma(a2s[j],sha,sca,1);        /* Gamma proposal */
        nprior+=dunif(a2star[j],0.0,mstep,1);
        oprior+=dunif(a2s[j],0.0,mstep,1);
        
        sha=gs[ncentres*6+states+ncentres+j];
        sca=bs[j]/sha;
        bstar[j]=rgamma(sha,sca);
        nprop+=dgamma(bstar[j],sha,sca,1);     /* Gamma proposal */
        sca=bstar[j]/sha;
        oprop+=dgamma(bs[j],sha,sca,1);        /* Gamma proposal */
        nprior+=dunif(bstar[j],0.0,bupper,1);
        oprior+=dunif(bs[j],0.0,bupper,1);
        
        sha=gs[ncentres*6+states+ncentres+states+j];
        sca=b2s[j]/sha;
        b2star[j]=rgamma(sha,sca);
        nprop+=dgamma(b2star[j],sha,sca,1);     /* Gamma proposal */
        sca=b2star[j]/sha;
        oprop+=dgamma(b2s[j],sha,sca,1);        /* Gamma proposal */
        nprior+=dunif(b2star[j],0.0,bupper,1);
        oprior+=dunif(b2s[j],0.0,bupper,1);
        
        sha=gs[ncentres*6+states+ncentres+states+ncentres+j];
        sca=d_zs[j]/sha;
        d_zstar[j]=rgamma(sha,sca);
        nprop+=dgamma(d_zstar[j],sha,sca,1);     /* Gamma proposal */
        sca=d_zstar[j]/sha;
        oprop+=dgamma(d_zs[j],sha,sca,1);        /* Gamma proposal */  
        nprior+=dunif(d_zstar[j],0.0,mdist,1);
        oprior+=dunif(d_zs[j],0.0,mdist,1);    
  
        nl= LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, astar, a2star, bstar, b2star, d_zstar, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
  
        if(runif(0.0,1.0)<exp(nl+nprior+oprop-ll-oprior-nprop)) {   /* Evaluate MH ratio.  Uniform priors cancel out */
          ll=nl;
          as[j]=astar[j];
          ap[ncentres*6+j] += pow(rr,(n-1 - g % (n)));
          bs[j]=bstar[j];
          ap[ncentres*6+states+ncentres+j] += pow(rr,(n-1 - g % (n)));
          a2s[j]=a2star[j];
          ap[ncentres*6+states+j] += pow(rr,(n-1 - g % (n)));
          b2s[j]=b2star[j];
          ap[ncentres*6+states+ncentres+states+j] += pow(rr,(n-1 - g % (n)));
          d_zs[j]=d_zstar[j];
          ap[ncentres*6+states+ncentres+states+ncentres+j] += pow(rr,(n-1 - g % (n)));
        }
      }
      /* Update Weibull parameters for exploratory states */
      for(j=ncentres; j < states; j++)  {
        for(jj=0; jj<(states);  jj++) {  
          astar[jj]=as[jj];
          bstar[jj]=bs[jj];
        }
        sha=gs[ncentres*6+j];
        sca=as[j]/sha;
        astar[j]=rgamma(sha,sca);
        nprop=dgamma(astar[j],sha,sca,1);     /* Gamma proposal */
        sca=astar[j]/sha;
        oprop=dgamma(as[j],sha,sca,1);        /* Gamma proposal */
        nprior=dunif(astar[j],0.0,mstep,1);
        oprior=dunif(as[j],0.0,mstep,1);
        
        sha=gs[ncentres*6+states+ncentres+j];
        sca=bs[j]/sha;
        bstar[j]=rgamma(sha,sca);
        nprop+=dgamma(bstar[j],sha,sca,1);     /* Gamma proposal */
        sca=bstar[j]/sha;
        oprop+=dgamma(bs[j],sha,sca,1);        /* Gamma proposal */
        nprior+=dunif(bstar[j],0.0,bupper,1);
        oprior+=dunif(bs[j],0.0,bupper,1);
  
        nl= LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, astar, a2s, bstar, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
  
        if(runif(0.0,1.0)<exp(nl+nprior+oprop-ll-oprior-nprop)) {   /* Evaluate MH ratio.  Uniform priors cancel out */
          ll=nl;
          as[j]=astar[j];
          ap[ncentres*6+j] += pow(rr,(n-1 - g % (n)));
          bs[j]=bstar[j];
          ap[ncentres*6+states+ncentres+j] += pow(rr,(n-1 - g % (n)));
        }
      }
    
    /* Update upsilon using random walk MH step */
    for (j=0; j<nexplore; j++)  {
    
      for (jj=0; jj<nexplore; jj++)  {
        upsilonstar[jj]=upsilons[jj];
      }
        
      if(Model_upsilon[j]==1) {    /* Only update if upsilon is in current model */
    
        invgs=1/gs[firstparms-4-nexplore+j];

        upsilonstar[j]=upsilons[j]+runif((-invgs),invgs);

        if(dunif(upsilonstar[j],0.0,1.0,0)>0) {   /* Check that 0 < upsilonstar < 1. Otherwise, reject. */
          nl= LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilonstar, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);

          if(runif(0.0,1.0)<exp(nl-ll)) {   /* Evaluate MH ratio.  Uniform priors and proposals cancel out */
            ll=nl;
            upsilons[j]=upsilonstar[j];
            ap[firstparms-4-nexplore+j] += pow(rr,(n-1 - g % (n)));
          }
        }
      }
    }
    
    /* Update phi_0s using random walk MH step */
    invgs=fmin((1/gs[firstparms-4]),PI);    /* Don't let proposal invgs get larger than PI */
    phi_0temp=phi_0s+runif((-invgs),invgs);
    phi_0star=(phi_0temp/(2*PI)-floor(phi_0temp/(2*PI)))*(2*PI);  /* Wrap phi_0 on the unit circle */

    nl = LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0star, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);

    if(runif(0.0,1.0)<exp(nl-ll)) {     /* Evaluate MH ratio. Uniform priors and proposals cancel out */
      ll=nl;
      phi_0s=phi_0star;
      ap[firstparms-4] += pow(rr,(n-1 - g % (n)));
    }
    
    w_n += pow(rr,(n-1 - g % (n)));   /* Calculate denominator for acceptance rate */
 
    /* Calculate acceptance rate every n iterations, and tune proposal parameters (gs) if g < adapt */   
    if((g+1) % n == 0) {

      for (j=0; j < (firstparms); j++) {
        arate[j] = ap[j] / w_n;
        if(g < ada) {
          gs[j] = 1/((1/gs[j])*pow(qq,(arate[j]-((j<(4*ncentres) || j>(6*ncentres-1)) ? targetaccept : targetcentreaccept))));    /* Tune gs for centre of attraction locations using targetcentreaccept */
        }
        ap[j] = 0.0;   /* Reset numerator of acceptance rate to zero */
      }
      /* Ensure 1/gs for centre of attraction location updates doesn't exceed maxdist */
      for (j=(4*ncentres); j<(6*ncentres); j++) {
        gs[j]=(((1/gs[j])>mdist) ? (1/mdist) : gs[j]);
      }
      /* Recalculate Mk based on new centredist */
      for(j=0; j<ncentres; j++) {
        centredist = 1/gs[4*ncentres+j];
        Mk[j]=GETMk(j, nsteps, X_zs, Y_zs, X_ts, Y_ts, centredist);
      } 
      for (j=firstparms; j < (firstparms+2*nsteps); j++) {
        arate[j] = ap[j] / w_n;
        if(g < ada) {
          gs[j] = 1/((1/gs[j])*pow(qq,(arate[j]-targetxyaccept)));    
        }
        ap[j] = 0.0;   /* Reset numerator of acceptance rate to zero */
      }  
      w_n=0.0;     /* Reset arate denominator to zero */
    }
    
    /* Model Updates */ 
      
      /* Update Model for q_z and eta_z */
      for(j=0; j<ncentres; j++)  {
      
        for(jj=0; jj<(ncentres); jj++)  {
          q_zstar[jj]=q_zs[jj];
          Model_qstar[jj]=Model_q[jj];
          eta_zstar[jj]=eta_zs[jj];
          Model_etastar[jj]=Model_eta[jj];
        }
      
        /* If q_z is not in the current model, propose to add q_z */
        if(Model_q[j]==0) {

          q_zstar[j]=rnorm(0,sqrt(taus));      /* Propose new q_z from prior distribution */
          Model_qstar[j]=1;                    /* Propose new model that includes q_z */
          
          nl=LIKE(eta_zs, m_zs, r_zs, q_zstar, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_qstar, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
            
          if(runif(0.0,1.0)<exp(nl-ll)) {  /* Evaluate MH ratio */
            ll=nl;
            q_zs[j]=q_zstar[j];
            Model_q[j] = Model_qstar[j];
            if(g<ada) {gs[3*ncentres+j]=newgs;}  /* If still adapting proposals, reset gs to newgs */
          }    
        } else  {                                  /* If q_z is in the current model, propose to remove q_z */

          q_zstar[j]=0;      
          Model_qstar[j]=0;                    /* Propose new model that removes q_z */

          nl=LIKE(eta_zs, m_zs, r_zs, q_zstar, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_qstar, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
          
          if(runif(0.0,1.0)<exp(nl-ll)) {   /* Evaluate MH ratio */
            ll=nl;
            q_zs[j]=q_zstar[j];
            Model_q[j] = Model_qstar[j];
          }
            
        }
        
        /* If eta_z is not in the current model, propose to add eta_z */
        if(Model_eta[j]==0) {

          eta_zstar[j]=runif(0.0,1.0);   /* Propose new eta_z from prior distribution */
          Model_etastar[j]=1;            /* Propose new model that includes eta_z */            

          nl=LIKE(eta_zstar, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_etastar, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
            
          if(runif(0.0,1.0)<exp(nl-ll)) {    /* Evaluate MH ratio.  Parameter/model priors and proposals cancel out */
            ll=nl;
            eta_zs[j]=eta_zstar[j];
            Model_eta[j] = Model_etastar[j];
            if(g<ada) {gs[j]=newgs;}           /* If still adapting proposals, reset gs to newgs */
          }
        } else {                               /* If eta_z is in the current model, propose to remove eta_z */

          eta_zstar[j]=0;   
          Model_etastar[j]=0;            /* Propose new model that removes eta_z */            

          nl=LIKE(eta_zstar, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilons, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_etastar, Model_q, Model_upsilon, nsteps, ncentres, ssidx, j_ti, mdist, mstep);
            
          if(runif(0.0,1.0)<exp(nl-ll)) {    /* Evaluate MH ratio.  Parameter/model priors and proposals cancel out */
            ll=nl;
            eta_zs[j]=eta_zstar[j];
            Model_eta[j] = Model_etastar[j];
          }
        }
      } 

      /* Update Model for upsilon */
      for(j=0; j<nexplore; j++) {
        for(jj=0; jj<nexplore; jj++) {
          upsilonstar[jj]=upsilons[jj];
          Model_upsilonstar[jj]=Model_upsilon[jj];
        }  
        /* If upsilon is not in current model, propose to add it */
        if(Model_upsilon[j]==0) {
        
          upsilonstar[j]=runif(0.0,1.0);   /* Propose new upsilon from prior distribution */
          Model_upsilonstar[j]=1;          /* Propose new model that includes upsilon */
        
          nl=LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilonstar, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilonstar, nsteps, ncentres, ssidx, j_ti, mdist, mstep);

          if(runif(0.0,1.0)<exp(nl-ll)) {   /* Evaluate MH ratio.  Parameter/model priors and proposals cancel out */
            ll=nl;
            upsilons[j]=upsilonstar[j];
            Model_upsilon[j] = Model_upsilonstar[j];
            if(g<ada) {gs[firstparms-4-nexplore+j]=newgs;}   /* If still adapting proposals, reset gs to newgs */
          }
        } else {                    /* If upsilon is in current model, propose to remove it */ 
        
          upsilonstar[j]=0.0;          /* Propose to remove upsilon */
          Model_upsilonstar[j]=0;      /* Propose new model that has upsilon removed */
        
          nl=LIKE(eta_zs, m_zs, r_zs, q_zs, X_zs, Y_zs, as, a2s, bs, b2s, d_zs, upsilonstar, phi_0s, sigma2_xs, sigma2_ys, X_ts, Y_ts, x_ti, y_ti, z_ts, Model_eta, Model_q, Model_upsilonstar, nsteps, ncentres, ssidx, j_ti, mdist, mstep);

          if(runif(0.0,1.0)<exp(nl-ll)) {     /* Evaluate MH ratio.  Parameter/model priors and proposals cancel out */
            ll=nl;
            upsilons[j]=upsilonstar[j];
            Model_upsilon[j] = Model_upsilonstar[j];
          }
        }
      } 
     
     /* Store current samples from the posterior distribution in vectors for output according to thin specification */ 
    if(((g+1) % th)==0)  {      
      loglike[count] = ll;    
      for (j=0; j<ncentres; j++) {
        Model[count*(ncentres*2+nexplore)+j] = Model_eta[j];
        Model[count*(ncentres*2+nexplore)+ncentres+j] = Model_q[j];
        a2[count*(ncentres)+j]=a2s[j];
        b2[count*(ncentres)+j]=b2s[j];
        d_z[count*(ncentres)+j]=d_zs[j];
      }
      for(j=0; j<nexplore; j++)  {
        Model[count*(ncentres*2+nexplore)+2*ncentres+j] = Model_upsilon[j];
      }
      
      sigma2_x[count]=sigma2_xs;
      sigma2_y[count]=sigma2_ys;
      
      for(j=0; j<nexplore; j++) {
        upsilon[count*(nexplore)+j]=upsilons[j];
      }
      
      phi_0[count]=phi_0s;
      tau[count]=taus;
      
      for (j=0; j < ncentres; j++)  {
        eta_z[count*ncentres+j]=eta_zs[j];
        m_z[count*ncentres+j]=m_zs[j];
        r_z[count*ncentres+j]=r_zs[j];
        q_z[count*ncentres+j]=q_zs[j];
        X_z[count*ncentres+j]=X_zs[j];
        Y_z[count*ncentres+j]=Y_zs[j];
        a[count*(states)+j]=as[j];
        b[count*(states)+j]=bs[j];
        centre_loc[count*ncentres+j]=centre_locs[j];
      }
      for (j=ncentres; j < states; j++)  {
        a[count*(states)+j]=as[j];
        b[count*(states)+j]=bs[j];
      } 
      
      for (j=0; j < nsteps; j++)  {
        X_t[count * nsteps+j]=X_ts[j];
        Y_t[count * nsteps+j]=Y_ts[j];
        z_t[count * nsteps+j]=z_ts[j];
      }
      
      for (j=0; j < (states)*(states); j++)  {
        psi[count*(states)*(states)+j]=psis[j];
      }
      
      count += 1;              /* Increment thinning count by 1 */
      if(*print_iteration_count==1) Rprintf("iteration %d \n",g+1);        /* Print iteration count if print_iteration_count = 1 */
    }              
  }   
  /* End Markov chain */

}     
/* End function MCMCloop */



/* FUNCTION DEFINITIONS */

/* Define function GETRHO for calcuting strength of bias (rho_z) to centres of attraction as a function of distance */
double GETRHO(double m_z, double r_z, double q_z, double distance, double mdistance)
{
  double logit, sdistance, rho_z;
  sdistance = distance/mdistance;
  logit = m_z + r_z*sdistance + q_z*pow(sdistance,2.0);
  rho_z = exp(logit)/(1+exp(logit));
  return(rho_z);
}

/* Define function GETSTEP for calculating distance between (X1,Y1) and (X0, Y0) */
double GETSTEP(double X1, double Y1, double X0, double Y0)
{
    double adj, op, step;    
    adj = X1-X0;
    op = Y1-Y0;
    step = sqrt(pow(adj,2.0) + pow(op,2.0));
    if(step==0.0)  {step=0.0000000000001;}
    return(step);
}

/* Define function GETDIR for calculating direction between (X1,Y1) and (X0,Y0) */
double GETDIR(double X1, double Y1, double X0, double Y0, double steplength)
{
    double adj, op, c, s, phi;    
    adj = X1-X0;
    op = Y1-Y0;
    c = adj/steplength;
    s = op/steplength;
    phi = acos(c);
    if(s<0.0) {phi = 2*PI-acos(c);}
    return(phi);
}

/* Define function DWCAUCHY for calculating wrapped Cauchy density */
double DWCAUCHY(double phi, double lambda, double rho)
{
    double dens;
    if(rho < 0 || rho >= 1.0) {
      dens = -HUGE_VAL;
    } else  {
    dens=log((1.0/(2.0*PI)) * (1 - pow(rho,2.0)) / (1+pow(rho,2.0) - 2*rho*cos(phi-lambda)));
    }
    return(dens);
}

/* Define function RMULTINOM to draw from a multinomial distribution */
void RMULTINOM(int n, double* prob, int K, int* rN)
{
    int k;
    double pp;
    double p_tot = 0.0;

    for(k = 0; k < K; k++) {
	   pp = prob[k];
	   p_tot += pp;
	   rN[k] = 0;
    }

    if(fabs((double) p_tot - (double) 0.) == (double) 0.)  {
      Rprintf("error: RMULTINOM probability sum should be >0, but is %f \n", (double) p_tot);
      return;
    } else {
    
     /* Generate the first K-1 obs. via binomials */
  
      for(k = 0; k < K-1; k++) {
        if(prob[k]) {
          pp = prob[k] / p_tot;
          rN[k] = ((pp < 1.) ? (int) rbinom((double) n,  pp) : n);
          n -= rN[k];
        }
        else rN[k] = 0;
        if(n <= 0) /* we have all*/ return;
        p_tot -= prob[k]; /* i.e. = sum(prob[(k+1):K]) */
      }
      rN[K-1] = n;
    }
    return;
}

/* Define function GETDENOM to calculate the sum of a vector */
double GETDENOM(double *prob, int dim)
{
  int j;
  double denom=0.0;
  
  for(j=0; j<dim; j++)  {
    denom+=prob[j];
  }
  return(denom);
}     

/* Define function LIKEOBS for calculating the N(zhat,sigma2) observation model likelihood for each time step */
double LIKEOBS(int ssidx1, int ssidx2, const double *j_ti, double X0, double X1, double Y0, double Y1, double *xti, double sigma2x, double *yti, double sigma2y)
{
    int s;
    double zhatx, zhaty, dens=0.0;
    for (s=(ssidx1-1); s < (ssidx2-1); s++)  {
      if(j_ti[s] != -1) {                         /* Only evaluate observation model likelihood if there are data for the interval */
        zhatx = (1-j_ti[s])*X0+j_ti[s]*X1;        /* Expected value for x_ti~Normal(zhatx,sigma2_x) observation model */
        zhaty = (1-j_ti[s])*Y0+j_ti[s]*Y1;        /* Expected value for y_ti~Normal(zhaty,sigma2_y) observation model */
        dens += dnorm(xti[s],zhatx,sqrt(sigma2x),1)+dnorm(yti[s],zhaty,sqrt(sigma2y),1);
      }
    }
    return(dens);
}

/* Define function LIKE to calculate the likelihood */
double LIKE(double *eta_z, double *m_z, double *r_z, double *q_z, double *X_z, double *Y_z, double *a, double *a2, double *b, double *b2, double *d_z, double *upsilon, double phi_0, double sigma2_x, double sigma2_y, double *X_ts, double *Y_ts, double *x_ti, double *y_ti, int *z_ts, int *Model_eta, int *Model_q, int *Model_upsilon, const int nsteps, int ncentres, const int *ssidx, const double *j_ti, const double mdist, double mstep)
{                                                                                                                                                                                                                                                                                                          
  int j, state;
  double step, phi, rho, delta_t, mu=0.0;
  double scale, shape;
  double liken  = 0.0;
  
  double prevphi=phi_0;
  double lambda;

  for(j=0; j < (nsteps-1); j++) {
  
    step = GETSTEP(X_ts[j+1],Y_ts[j+1],X_ts[j],Y_ts[j]);
    phi = GETDIR(X_ts[j+1],Y_ts[j+1],X_ts[j],Y_ts[j],step);

    state = z_ts[j+1]-1;
    
    /* Specify parameters for wrapped Cauchy distribution based on whether exploratory state or not */
    if(state<ncentres)  {
    
      delta_t=GETSTEP(X_z[state],Y_z[state],X_ts[j],Y_ts[j]);
      mu=GETDIR(X_z[state],Y_z[state],X_ts[j],Y_ts[j],delta_t);
      rho = GETRHO(m_z[state], r_z[state], (q_z[state]*Model_q[state]), delta_t, mdist);
      
      if(delta_t < d_z[state]) {
        scale=a2[state];
        shape=b2[state];
      } else  {
        scale=a[state];
        shape=b[state];
      }
      
      if(fabs(prevphi-mu)>PI) {
        if(prevphi>PI)  {
          prevphi= -(2*PI-prevphi);
        }
        if(mu>PI)  {
          mu = -(2*PI-mu);
        }
      }  
      lambda=eta_z[state]*Model_eta[state]*prevphi+(1-eta_z[state]*Model_eta[state])*mu;
      if(lambda<0) {lambda=2*PI+lambda;}
    
    } else {
      rho = upsilon[state-ncentres]*Model_upsilon[state-ncentres];
      lambda=prevphi;
      scale=a[state];
      shape=b[state];  
    }

    if(step < mstep)  {
      liken += DWCAUCHY(phi,lambda,rho) + dweibull(step,shape,scale,1) + LIKEOBS(ssidx[j],ssidx[j+1],j_ti,X_ts[j],X_ts[j+1],Y_ts[j],Y_ts[j+1],x_ti,sigma2_x,y_ti,sigma2_y);
    } else {
      liken = -HUGE_VAL;
    }
    prevphi=phi;
  }
  liken = ((isfinite(liken)) ? liken : (-HUGE_VAL));
  return(liken);
}

/* Define function LIKEXY to update locations (X_ts, Y_ts) */
double LIKEXY(int i, int blk, double *eta_z, double *m_z, double *r_z, double *q_z, double *X_z, double *Y_z, double *a, double *a2, double *b, double *b2, double *d_z, double *upsilon, double phi_0, double sigma2_x, double sigma2_y, double *X_ts, double *Y_ts, double *x_ti, double *y_ti, int *z_ts, int *Model_eta, int *Model_q, int *Model_upsilon, const int nsteps, int ncentres, const int *ssidx, const double *j_ti, const double mdist, double mstep)
{
  int j, state;
  double step, phi, rho, delta_t, mu=0.0;
  double scale, shape;
  double liken = 0.0;
  
  double prevphi=phi_0;
  double lambda;

  if(i>1) {
    step = GETSTEP(X_ts[i-1],Y_ts[i-1],X_ts[i-2],Y_ts[i-2]);
    prevphi = GETDIR(X_ts[i-1],Y_ts[i-1],X_ts[i-2],Y_ts[i-2],step);
  }
       
  for(j=i-1; (j < i+blk+2) && (j<nsteps-1); j++) {
  
    if((j>=0)) {
    
      step = GETSTEP(X_ts[j+1],Y_ts[j+1],X_ts[j],Y_ts[j]);
      phi = GETDIR(X_ts[j+1],Y_ts[j+1],X_ts[j],Y_ts[j],step);

      state = z_ts[j+1]-1;
      
      /* Specify parameters for wrapped Cauchy distribution based on whether exploratory state or not */
      if(state<ncentres)  {
    
        delta_t=GETSTEP(X_z[state],Y_z[state],X_ts[j],Y_ts[j]);
        mu=GETDIR(X_z[state],Y_z[state],X_ts[j],Y_ts[j],delta_t);
        rho = GETRHO(m_z[state], r_z[state], (q_z[state]*Model_q[state]), delta_t, mdist);
        
        if(delta_t < d_z[state]) {
          scale=a2[state];
          shape=b2[state];
        } else  {
          scale=a[state];
          shape=b[state];
        }
      
        if(fabs(prevphi-mu)>PI) {
          if(prevphi>PI)  {
            prevphi= -(2*PI-prevphi);
          }
          if(mu>PI)  {
            mu = -(2*PI-mu);
          }
        }  
        lambda=eta_z[state]*Model_eta[state]*prevphi+(1-eta_z[state]*Model_eta[state])*mu;
        if(lambda<0) {lambda=2*PI+lambda;}
      
      } else {
        rho = upsilon[state-ncentres]*Model_upsilon[state-ncentres]; 
        lambda=prevphi;
        scale=a[state];
        shape=b[state];
      }
      if(step < mstep)  {
        liken += DWCAUCHY(phi,lambda,rho) + dweibull(step,shape,scale,1) + LIKEOBS(ssidx[j],ssidx[j+1],j_ti,X_ts[j],X_ts[j+1],Y_ts[j],Y_ts[j+1],x_ti,sigma2_x,y_ti,sigma2_y);
      } else {
        liken = -HUGE_VAL;
      }
      prevphi=phi;
    }
  }
  liken = ((isfinite(liken)) ? liken : (-HUGE_VAL));
  return(liken);
}

/* Define function GETZ to update state assignments (z_ts) for each time step from the full conditional posterior distribution */
void GETZ(double *eta_zs, double *m_zs, double *r_zs, double *q_zs, double *X_zs, double *Y_zs, double *as, double *a2s, double *bs, double *b2s, double *d_zs, double *upsilons, double phi_0s, double *X_ts, double *Y_ts, int *z_ts, int *centre_locs, double *psis, int *Model_eta, int *Model_q, int *Model_upsilon, const int nsteps, int ncentres, int states, const double mdist, double *zdens)
{
  int j, jj, k, state;
  int rN[states];
  double step, phi, rho, delta_t=0.0, mu=0.0;
  double shape, scale;
  double pi[states],p[states];               /* Parameters of the full conditional categorical distribution for z_0 and z_t */
  double newp[states];
  double mp[states];
  
  double prevphi=phi_0s;
  double lambda;
  
  for(jj=0; jj<nsteps; jj++)  {
    zdens[jj]=0;
  }
  
  /* Draw initial state z_0 */
  for (jj=0; jj < (states); jj++) {                
    pi[jj]=psis[z_ts[1]-1+jj*(states)];
  }
  RMULTINOM(1,pi,(states),rN);  
  for (k=0; k < (states); k++) {
    if((k<ncentres && centre_locs[k]==0))  {           /* Ensure that locations corresponding to centres of attraction are assigned to the appropriate centre of attraction state   */
      z_ts[0] = k+1;
      k = states;
    } else  {
      if(rN[k]==1)  {
        z_ts[0]=k+1;
        zdens[0]=log(pi[k]/(GETDENOM(pi,states)));
      }
    }
  }
  
  /* Update remaining z_t for t=1,...,T */
  for(j=0; j < (nsteps-1); j++) {

    step = GETSTEP(X_ts[j+1],Y_ts[j+1],X_ts[j],Y_ts[j]);
    phi = GETDIR(X_ts[j+1],Y_ts[j+1],X_ts[j],Y_ts[j],step);
      
    for (jj=0; jj < (states); jj++) {
    
      state = jj;
      
      /* Specify parameters for wrapped Cauchy distribution based on whether exploratory state or not */
      if(state<ncentres)  {
        
        delta_t=GETSTEP(X_zs[state],Y_zs[state],X_ts[j],Y_ts[j]);
        mu=GETDIR(X_zs[state],Y_zs[state],X_ts[j],Y_ts[j],delta_t);
        rho = GETRHO(m_zs[state], r_zs[state], (q_zs[state]*Model_q[state]), delta_t, mdist);
        
        if(delta_t < d_zs[state]) {
          scale=a2s[state];
          shape=b2s[state];
        } else  {
          scale=as[state];
          shape=bs[state];
        }

        if(fabs(prevphi-mu)>PI) {
          if(prevphi>PI)  {
            prevphi= -(2*PI-prevphi);
          }
          if(mu>PI)  {
            mu = -(2*PI-mu);
          }
        }  
        lambda=eta_zs[state]*Model_eta[state]*prevphi+(1-eta_zs[state]*Model_eta[state])*mu;
        if(lambda<0) {lambda=2*PI+lambda;}
      
      } else {
        rho = upsilons[state-ncentres]*Model_upsilon[state-ncentres];
        lambda=prevphi;
        scale=as[state];
        shape=bs[state];
      }
          
      newp[jj] = exp(DWCAUCHY(phi,lambda,rho) + dweibull(step,shape,scale,1));

      if(j==(nsteps-2))  {
        mp[jj]=psis[jj+(z_ts[j]-1)*(states)] * newp[jj];
      } else  {
        mp[jj]=psis[jj+(z_ts[j]-1)*(states)] * newp[jj]* psis[z_ts[j+2]-1+jj*(states)];
      }
    }
    prevphi=phi;
    
    for (jj=0; jj < (states); jj++) {                /* Normalize so state probabilities sum to 1 */
        p[jj]= (isfinite(mp[jj]) ? mp[jj] : 0.0);
    }
    RMULTINOM(1,p,(states),rN);                   /* Draw new state from full conditional (categorical) distribution for z_t */
    for (k=0; k < (states); k++) {
      if((k<ncentres && centre_locs[k]==(j+1)))  {           /* Ensure that locations corresponding to centres of attraction are assigned to the appropriate centre of attraction state   */
        z_ts[j+1] = k+1;
        k = states;
      } else  {
        if(rN[k]==1)  {
          z_ts[j+1]=k+1;
          zdens[j+1]=log(p[k]/(GETDENOM(p,states)));
        }
      }
    }
  }    
}

/* Define function ZPRIOR to calculate the prior density for state assingments (z_t) */
double ZPRIOR(double *psi, int *z_t, int states, int nsteps)
{
  int i;
  double dens=0.0;
  for(i=1; i<nsteps; i++) {
    dens += log(psi[(z_t[i]-1)+(z_t[i-1]-1)*(states)]);    
  }
  return(dens);
}    

/* Define function GETPSI for updating transition probabilities (psi) by drawing from the full conditional posterior distribution */
void GETPSI(double *pvect, int *z_t, int nsteps, int id, int dim) 
{
  int k, kk, nu[dim];
  
  for (kk=0; kk < dim; kk++)  {
    nu[kk]=0;
  }
    
  for (kk=0; kk < dim; kk++)  {
    for (k=0; k < (nsteps-1); k++)  {
        if(z_t[k+1]==(kk+1) && z_t[k]==id)  {
          nu[kk] += 1;
        }
    }
  }
      
  double xx[dim], sumx=0.0;
  for (k = 0; k < dim; k++) {
    xx[k]=rgamma((nu[k]+1),1.0);
    sumx += xx[k];
  }

  for (k = 0; k < dim; k++) {
    pvect[k]=xx[k]/sumx;
  }
}

/* Define function DINVGAMMA(value,shape,scale) for the inverse gamma distribution */
double DINVGAMMA(double xx, double shape, double scale)
{
  double density, logdensity;  
  logdensity = shape * log(scale) - lgammafn(shape) - (shape + 1) * log(xx) - (scale / xx);
  density = exp(logdensity);
  return(density);
}

/* Define function PNPOLY to determine whether coordinates (testx, testy) lie within the nvert-sided polygon defined by (vertx,verty) */
int PNPOLY(int nvert, double *vertx, double *verty, double testx, double testy)
{
  int i, j, c = 0;
  for (i = 0, j = nvert-1; i < nvert; j = i++) {
    if ( ((verty[i]>testy) != (verty[j]>testy)) &&
	 (testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
       c = !c;
  }
  return c;
}

/* Propose new position (newpos) for centre of attraction (X_z, Y_z) from discrete uniform distribution over all positions within specified distance (centredist) of current centre of attraction */
int PROPCENTRE(int centre, int *z_ts, int nsteps, double *X_z, double *Y_z, double *X_t, double *Y_t, double centredist, int centreloc)
{
  int j, count=0, timesteps[nsteps], newpos=centreloc;
  
  for (j=0; j<centreloc; j++)  {
    if(GETSTEP(X_t[j], Y_t[j], X_z[centre], Y_z[centre])<centredist)  {
        timesteps[count] = j;
        count += 1; 
    }
  }
  
  for (j=(centreloc+1); j<nsteps; j++)  {
    if(GETSTEP(X_t[j], Y_t[j], X_z[centre], Y_z[centre])<centredist)  {
        timesteps[count] = j;
        count += 1; 
    }
  }
  
  int rN[count];
  double temp[count];
  for(j=0; j < count; j++)  {
    temp[j]=1.0/count;
  }
  RMULTINOM(1,temp,count,rN);
  for (j=0; j < count; j++) {
    if(rN[j]==1)  {
      newpos=timesteps[j];
    }
  }
  return(newpos);
}

/* Calculate number of locations (X_t, Y_t) within some distance of centre of attraction (X_z, Y_z) */
int GETMk(int centre, int nsteps, double *X_z, double *Y_z, double *X_t, double *Y_t, double centredist)
{
  int jj, m_k = -1;
  
  for(jj=0; jj<nsteps; jj++)  {
    if(GETSTEP(X_t[jj], Y_t[jj], X_z[centre], Y_z[centre])<centredist) {
      m_k += 1;
    }
  }
  return(m_k);
}