/****************************************************************************
 *                                                                          *
 *     Loki - Programs for genetic analysis of complex traits using MCMC    *
 *                                                                          *
 *             Simon Heath - Rockefeller University                         *
 *                                                                          *
 *                       November 1997                                      *
 *                                                                          *
 * loki_tlmoves.c:                                                          *
 *                                                                          *
 * Lots of the more weird update moves                                      *
 *                                                                          *
 * Copyright (C) Simon C. Heath 1997, 2000, 2002                            *
 * This is free software.  You can distribute it and/or modify it           *
 * under the terms of the Modified BSD license, see the file COPYING        *
 *                                                                          *
 ****************************************************************************/

#include <stdlib.h>
#ifdef USE_DMALLOC
#include <dmalloc.h>
#endif
#include <math.h>
#include <stdio.h>
#include <float.h>
#ifndef DBL_MAX
#define DBL_MAX MAXDOUBLE
#endif

#include "ranlib.h"
#include "utils.h"
#include "loki.h"
#include "loki_peel.h"
#include "loki_tlmoves.h"
#include "handle_res.h"
#include "sample_cens.h"

static int *perm;
static double *prior,*new_freq;

static double safe_exp(double x)
{
	static double max_arg;
	static char f=0;
	
	if(!f) {
		max_arg=log(DBL_MAX);
		f=1;
	}
	if(x>max_arg) return DBL_MAX;
	if(x<-max_arg) return 0.0;
	return exp(x);
}

static void Adjust_for_TL(const int tl,const double z)
{
	int i,j,k,type;
	double *eff;
	int *gt;
	
	eff=tlocus[tl].eff[0];
	gt=tlocus[tl].locus.gt;
	type=models[0].var.type;
	if(type&ST_CONSTANT)	{
		for(i=0;i<ped_size;i++) if(id_array[i].res[0]) {
			k=gt[i]-1;
			if(k) id_array[i].res[0][0]+=z*eff[k-1];
		}
	} else for(i=0;i<ped_size;i++) if(id_array[i].res[0]) {
		k=gt[i]-1;
		if(k) for(j=0;j<id_array[i].n_rec;j++) id_array[i].res[0][j]+=z*eff[k-1];
	}
}

static void Adjust_Mean(const double adj)
{
	int i,j,type;

	type=models[0].var.type;
	if(type&ST_CONSTANT)	{
		for(i=0;i<ped_size;i++) if(id_array[i].res[0]) id_array[i].res[0][0]+=adj;
	} else for(i=0;i<ped_size;i++) if(id_array[i].res[0]) for(j=0;j<id_array[i].n_rec;j++) id_array[i].res[0][j]+=adj;
	grand_mean[0]-=adj;
}

static double interval_size(int j,int k,int link,int *pm)
{
	int k2,k1;
	double s=0.0,x;
	
	for(k2=0;k2<=sex_map;k2++)	{
		if(!j) {
			k1=pm[0];
			if(k1<0) x=tlocus[-1-k1].locus.pos[k2];
			else x=marker[k1].locus.pos[k2];
			s+=x-linkage[link].r1[k2];
		} else if(j==k) {
			k1=pm[k-1];
			if(k1<0) x=tlocus[-1-k1].locus.pos[k2];
			else x=marker[k1].locus.pos[k2];
			s+=linkage[link].r2[k2]-x;
		} else {
			k1=pm[j];
			if(k1<0) x=tlocus[-1-k1].locus.pos[k2];
			else x=marker[k1].locus.pos[k2];
			k1=pm[j-1];
			if(k1<0) x-=tlocus[-1-k1].locus.pos[k2];
			else x-=marker[k1].locus.pos[k2];
			s+=x;
		}
	}
	return s;
}

static double check_intervals(int j,int k,int link,int *pm,double *p,double move_p)
{
	int k1;
	double z,z1;
	
	z=(1.0-move_p)*.5;
	p[1]=move_p;
	/* Check if we can move to neighbouring intervals */
	p[0]=j?z:0.0;
	p[2]=j<k?z:0.0;
	/* Are there intervals outside the markers? */
	if(j==1) {
		k1=pm[0];
		if(k1<0) z=tlocus[-1-k1].locus.pos[0];
		else z=marker[k1].locus.pos[0];
		if(z<=linkage[link].r1[0]) p[0]=0.0;
	}
	if(j==(k-1)) {
		k1=pm[k];
		if(k1<0) z=tlocus[-1-k1].locus.pos[0];
		else z=marker[k1].locus.pos[0];
		if(z>=linkage[link].r2[0]) p[2]=0.0;
	}
	if(p[0]>0.0) p[0]*=interval_size(j-1,k,link,pm);
	p[1]*=(z1=interval_size(j,k,link,pm));
	if(p[2]>0.0) p[2]*=interval_size(j+1,k,link,pm);
	z=p[0]+p[1]+p[2];
	for(k1=0;k1<3;k1++) p[k1]/=z;
	return z1;
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "sample_mpos"
void sample_mpos(const int link) 
{
	int i,j,locus,k2=-1,k3,ids,idd,sx,**seg,s,s1;
	double pp[2][50],ct1[2][50],ct2[2][50],x,x1,r,z,z1;
	struct Locus *loc;
	
	get_locuslist(perm,link,&k3,0);
	gnu_qsort(perm,(size_t)k3,(size_t)sizeof(int),cmp_loci_pos);
	for(i=0;i<k3;i++) {
		locus=perm[i];
		if(locus<0) {
			if(!(tlocus[-1-locus].locus.flag&LOCUS_SAMPLED)) break;
		} else { 
			if(!(marker[locus].locus.flag&LOCUS_SAMPLED)) break;
		}
		ct1[0][i]=ct1[1][i]=0.0;
		ct2[0][i]=ct2[1][i]=0.0;
	}
	if(i<k3) return;
	z=ct1[0][0]+ct1[1][0];
	z1=ct2[0][0]+ct2[1][0];
	for(i=0;i<ped_size;i++) {
		ids=id_array[i].sire;
		idd=id_array[i].dam;
		if(!ids) continue;
		for(sx=0;sx<2;sx++) {
			for(j=0;j<k3;j++) {
				locus=perm[j];
				loc=locus<0?&tlocus[-1-locus].locus:&marker[locus].locus;
				seg=loc->seg;
				x=loc->pos[sx];
				k2=seg[sx][i];
				if(k2<0) {
					if(j) {
						r=.5*(1.0-exp(.02*(x1-x)));
						pp[0][j]=pp[0][j-1]*(1.0-r)+pp[1][j-1]*r;
						pp[1][j]=pp[1][j-1]*(1.0-r)+pp[0][j-1]*r;
					} else pp[0][j]=pp[1][j]=0.5;
				} else {
#ifdef DEBUG
					if(k2<0 || k2>1) ABT_FUNC("OOOOK!\n");
#endif
					pp[k2][j]=1.0;
					pp[1-k2][j]=0.0;
				}
				x1=x;
			}
			if(k2<0) {
				z=ranf()*(pp[0][k3-1]+pp[1][k3-1]);
				s=(z<=pp[0][k3-1]?0:1);
			} else s=k2;
			for(j=k3-2;j>=0;j--) {
				s1=s;
				x1=x;
				locus=perm[j];
				loc=locus<0?&tlocus[-1-locus].locus:&marker[locus].locus;
				seg=loc->seg;
				x=loc->pos[sx];
				k2=seg[sx][i];
				if(k2<0) {
					r=.5*(1.0-exp(.02*(x-x1)));
					pp[s1][j]*=1.0-r;
					pp[1-s1][j]*=r;
					z=ranf()*(pp[0][j]+pp[1][j]);
					s=(z<=pp[0][j]?0:1);
				} else {
					s=k2;
				}
				if(s!=s1) ct1[sx][j]+=1.0;
				else ct2[sx][j]+=1.0;
			}
		}
	}
	if(sex_map) {
		for(sx=0;sx<2;sx++) {
			for(j=0;j<k3;j++) {
				locus=perm[j];
				if(!j) {
					if(locus<0) {
						x=tlocus[-1-locus].locus.pos[sx];
					} else {
						x=marker[locus].locus.pos[sx];
					}
				} else {
					do {
						r=genbet(ct1[sx][j-1]+1.0,ct2[sx][j-1]+1.0);
					} while(r>=0.5);
					x-=50.0*log(1.0-2.0*r);
					if(locus<0) {
						tlocus[-1-locus].locus.pos[sx]=x;
					} else {
						marker[locus].locus.pos[sx]=x;
					}
				}
			}
		}
	} else {
		for(j=0;j<k3;j++) {
			locus=perm[j];
			if(!j) {
				if(locus<0) {
					x=tlocus[-1-locus].locus.pos[0];
				} else {
					x=marker[locus].locus.pos[0];
				}
			} else {
				z=ct1[0][j-1]+ct1[1][j-1];
				z1=ct2[0][j-1]+ct2[1][j-1];
				printf("AA: %g\n",z/(z+z1));
				do {
					r=genbet(ct1[0][j-1]+ct1[1][j-1]+1.0,ct2[0][j-1]+ct2[1][j-1]+1.0);
				} while(r>=0.5);
/*				x-=50.0*log(1.0-2.0*r);
				if(locus<0) {
					tlocus[-1-locus].locus.pos[0]=x;
					tlocus[-1-locus].locus.pos[1]=x;
				} else {
					marker[locus].locus.pos[0]=x;
					marker[locus].locus.pos[1]=x;
				} */
			}
		}
	}
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "Sample_LinkageGroup"
/* Sample genotypes for all marker loci in linkage group - trait loci are sampled during
 * movement step - see Sample_TL_Position() */
void Sample_LinkageGroup(const int link,struct peel_mem *work,int si)
{
	int i,j,k1,k2,k3,*perm1;
	
#ifdef DEBUG
	if((*debug_level)&4) (void)printf("[S:%d]",link);
	(void)fflush(stdout);
#endif
/*   	sample_mpos(link); */
	k2=linkage[link].n_markers+n_tloci;
	perm1=perm+k2;
	get_locuslist(perm1,link,&k3,0);
	for(i=0;i<k3;i++) perm[i]=perm1[i];
	gnu_qsort(perm1,(size_t)k3,(size_t)sizeof(int),cmp_loci_pos);
	gen_perm(perm,k3);
	for(i=0;i<k3;i++) {
		k1=perm[i];
		for(j=0;j<k3;j++) if(perm1[j]==k1) break;
		if(k1>=0) {
			(void)peel_locus(perm1,j,k3,1,work,si);
		}
	}
#ifdef DEBUG
	if((*debug_level)&4) {
		(void)fputc('*',stdout);
		(void)fflush(stdout);
	}
#endif
}

double calc_tl_like(const int tl,int *perm,const int sflag,struct peel_mem *work,int si)
{
	int i,k,link;
	double l;
	
	link=tlocus[tl].locus.link_group;
	if(link<0) {
		perm[0]= -(tl+1);
		i=0;
		k=1;
	} else {
		get_locuslist(perm,link,&k,0);
		gnu_qsort(perm,(size_t)k,(size_t)sizeof(int),cmp_loci_pos);
		for(i=0;i<k;i++) if(perm[i]== -(tl+1)) break;
	}
	l=peel_locus(perm,i,k,sflag,work,si);
	return l;
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "TL_Alloc"
void TL_Alloc(void)
{
	int k;
	
	k=n_markers+max_tloci;
	if(!k) k++;
	if(!(perm=malloc(k*2*sizeof(int)))) ABT_FUNC(MMsg);
	if(!(new_freq=malloc(2*(n_genetic_groups+n_links+1)*sizeof(double)))) ABT_FUNC(MMsg);
	prior=new_freq+n_genetic_groups*2;
}

void TL_Free(void)
{
	if(perm) free(perm);
	if(new_freq) free(new_freq);
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "Flip_TL_Alleles"
#ifdef DEBUG
void Flip_TL_Alleles(const int tl,int si,struct peel_mem *work)
#else
void Flip_TL_Alleles(const int tl)
#endif
{
	double z,z1,*p;
	int i;
#ifdef DEBUG
	double l,l1;
	
	l=calc_tl_like(tl,perm,0,work,si);
#endif
	if(tlocus[tl].locus.flag&LOCUS_SAMPLED) {
		Adjust_for_TL(tl,1.0);
		tlocus[tl].locus.flag&=~LOCUS_SAMPLED;
	}
	p=tlocus[tl].eff[0];
	z=p[0];
	z1=p[1];
	Adjust_Mean(-z1); 
	p[1]=-z1;
	p[0]=z-z1;
	for(i=0;i<n_genetic_groups;i++) {
		p=tlocus[tl].locus.freq[i];
		p[0]=1.0-p[0];
		p[1]=1.0-p[1];
	}
#ifdef DEBUG
	l1=calc_tl_like(tl,perm,0,work,si);
	if(fabs(l-l1)>1.0e-8) {
		fprintf(stderr,"%g %g  %g\n",l,l1,l-l1);
		ABT_FUNC("Likelihood mismatch\n");
	}
#endif
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "Sample_TL_Position"
/* Sample a new position for trait locus.  Allows moves to be made between
 * linkage groups (including to and from unlinked).
 * BIGMOVE_PROB is the probability of proposing a large move (uniform over entire genome)
 * vs. a small move (current location +/- 1 interval)
 * If unlinked then a move to linked is always attempted.
 */
void Sample_TL_Position(const int tl,struct peel_mem *work,int si)
{
	int i,j,j2,k,k1,k2,fg,link,oldlink,step;
	double l,l1,old_pos[2],map_length,z,z1,z2,r,*prob,ttm,p_i[3],p_i1[3],s1,s2;

	/* In the case of no linkage groups,
	 just sample genotypes for trait locus and return */
	if(!n_links) {
		(void)calc_tl_like(tl,perm,1,work,si);
		return;
	}
	prob=prior+n_links+1;
	/* Set up prior probs. for chromosomes (and unlinked region) */
	/* Note that we use the average of sex-specific maps */
	for(i=0;i<=n_links;i++) prior[i]=0.0;
	ttm=0.0;
	for(k=0;k<=sex_map;k++)	{
		z=total_maplength[k];
		ttm+=z;
		for(i=0;i<n_links;i++) {
			map_length=linkage[i].r2[k]-linkage[i].r1[k];
			z-=map_length;
			prior[i]+=map_length;
		}
		prior[i]+=z;
	}
	if(tlocus[tl].locus.flag&TL_LINKED) {
		link=tlocus[tl].locus.link_group;
		old_pos[0]=tlocus[tl].locus.pos[0];
		old_pos[1]=tlocus[tl].locus.pos[1];
	} else {
		link= -1;
		old_pos[0]=old_pos[1]=0.0;
	}
	l=calc_tl_like(tl,perm,0,work,si);
#ifdef DEBUG
	if(l== -DBL_MAX) {
		fprintf(stderr,"Trait locus %d\n",tl);
		ABT_FUNC("Internal error - zero probability at start of step\n");
	}
#endif
	oldlink=link;
	z1=1.0;
	/* If unlinked always attempt a 'big move', otherwise do so with probability BIGMOVE_PROB  */
	/* A 'big' move can be to anywhere in the genome (except that if unlinked can not jump to */
	/* another unlinked location) */
	if(link== -1 || (BIGMOVE_PROB>0.0 && ranf()<=BIGMOVE_PROB))	{
		step=5;
		z=0.0;
		for(i=0;i<=n_links;i++)	{
			z+=prior[i];
			prob[i]=z;
		}
		if(link== -1) { /* If already unlinked, prevent choosing unlinked again */
			z-=prior[n_links];
			prob[n_links]=z;
			j=n_links;
		} else j=n_links+1;
#ifdef DEBUG
		if(z<=0.0) ABT_FUNC("Internal error - no possible move\n");
#endif
		do {
			r=z*safe_ranf();
			for(i=0;i<j;i++) if(prior[i]>0.0 && r<=prob[i]) break;
		} while(i==j);
		link=i;
		if(link==n_links)	{
			link= -1;
			tlocus[tl].locus.pos[0]=tlocus[tl].locus.pos[1]=0.0;
		} else {
			if(link) r-=prob[link-1]; /* Get position within the chromosome we've landed on relative to left of linkage map */
			/* Get list of loci on linkage group (excluding current locus) */
			fg=tlocus[tl].locus.flag;
			tlocus[tl].locus.flag&=~TL_LINKED;
			get_locuslist(perm,link,&k,0);
			gnu_qsort(perm,(size_t)k,(size_t)sizeof(int),cmp_loci_pos);
			tlocus[tl].locus.flag=fg;
			if(sex_map)	{ /* Find interval */
				for(j=0;j<k;j++) {
					k1=perm[j];
					if(k1<0)	{
						z=tlocus[-1-k1].locus.pos[0]-linkage[link].r1[0];
						z+=tlocus[-1-k1].locus.pos[1]-linkage[link].r1[1];
					} else {
						z=marker[k1].locus.pos[0]-linkage[link].r1[0];
						z+=marker[k1].locus.pos[1]-linkage[link].r1[1];
					}
					if(z>=r) break;
				}
				/* Sample male and female positions (independently) within chosen interval */
				for(k2=0;k2<2;k2++) {
					if(j<k) {
						k1=perm[j];
						if(k1<0) z=tlocus[-1-k1].locus.pos[k2];
						else z=marker[k1].locus.pos[k2];
					} else z=linkage[link].r2[k2];
					if(j)	{
						k1=perm[j-1];
						if(k1<0) z2=tlocus[-1-k1].locus.pos[k2];
						else z2=marker[k1].locus.pos[k2];
					} else z2=linkage[link].r1[k2];
					tlocus[tl].locus.pos[k2]=z2+(z-z2)*safe_ranf();
				}
			} else tlocus[tl].locus.pos[0]=tlocus[tl].locus.pos[1]=r+linkage[link].r1[0];
		}
		tlocus[tl].locus.link_group=link;
		if(link!=oldlink)	{
			if(link<0 && oldlink>=0) z1=ttm/(BIGMOVE_PROB*(ttm-prior[n_links]));
			else if(link>=0 && oldlink<0) z1=BIGMOVE_PROB*(ttm-prior[n_links])/ttm;
		}
		if(link<0) {
			tlocus[tl].locus.flag&=~TL_LINKED;
			tlocus[tl].locus.flag|=TL_UNLINKED;
		} else {
			tlocus[tl].locus.flag&=~TL_UNLINKED;
			tlocus[tl].locus.flag|=TL_LINKED;
		}
	} else { /* 'Small' move - move at most 1 interval from current location */
		step=4;
		get_locuslist(perm,link,&k,0);
		gnu_qsort(perm,(size_t)k,(size_t)sizeof(int),cmp_loci_pos);
		for(j=0;j<k;j++) if(-1-perm[j]==tl) break;
#ifdef DEBUG
		if(j==k) ABT_FUNC("Internal error - couldn't find trait locus in linkage group\n");
#endif
		for(j2=j+1;j2<k;j2++) perm[j2-1]=perm[j2];
		k--;
		s1=check_intervals(j,k,link,perm,p_i,SMALLMOVE_P);
		/* Pick an interval */
		do {
			z2=ranf();
			z=0.0;
			for(k2=0;k2<3;k2++) if(p_i[k2]>0.0)	{
				z+=p_i[k2];
				if(z2<=z) break;
			}
		} while(k2==3);
		/* Have we changed intervals? */
		if(k2!=1) {
			j2=j+k2-1;
			/* Compute probs. for new interval */
			s2=check_intervals(j2,k,link,perm,p_i1,SMALLMOVE_P);
			/* Compute proposal probability */
			z1=(s2/s1)*p_i1[2-k2]/p_i[k2];
		} else j2=j;
		/* Sample new positions */
		for(k2=0;k2<=sex_map;k2++)	{
			if(j2) {
				k1=perm[j2-1];
				if(k1<0) z=tlocus[-1-k1].locus.pos[k2];
				else z=marker[k1].locus.pos[k2];
			} else z=linkage[link].r1[k2];
			if(j2<k) {
				k1=perm[j2];
				if(k1<0) z2=tlocus[-1-k1].locus.pos[k2];
				else z2=marker[k1].locus.pos[k2];
			} else z2=linkage[link].r2[k2];
			tlocus[tl].locus.pos[k2]=z+safe_ranf()*(z2-z);
		} 
		if(!sex_map) tlocus[tl].locus.pos[1]=tlocus[tl].locus.pos[0];
	}
#ifdef DEBUG
	if((*debug_level)&4) (void)printf("[%d:%d]",step,tl);
#endif
	move_stats[step].n++;
	l1=calc_tl_like(tl,perm,0,work,si);
	if(l1== -DBL_MAX) z= -1.0;
	else z=safe_exp(l1-l)*z1;
	i=0;
 	if(ranf()>=z) {
		i=1;
		tlocus[tl].locus.pos[0]=old_pos[0];
		tlocus[tl].locus.pos[1]=old_pos[1];
		link=oldlink;
		tlocus[tl].locus.link_group=link;
		if(link<0) {
			tlocus[tl].locus.flag&=~TL_LINKED;
			tlocus[tl].locus.flag|=TL_UNLINKED;
		} else {
			tlocus[tl].locus.flag&=~TL_UNLINKED;
			tlocus[tl].locus.flag|=TL_LINKED;
		}
#ifdef DEBUG
		if((*debug_level)&4) (void)fputc('F',stdout);
#endif
	} else {	
#ifdef DEBUG
		if((*debug_level)&4) (void)fputc('S',stdout);
#endif
		move_stats[step].success++;
	}
	/* Always sample genotypes before returning */
#ifdef DEBUG
	z=calc_tl_like(tl,perm,1,work,si);
	if(z== -DBL_MAX) ABT_FUNC("Internal error - zero probability at end of Move Step\n");
	if((*debug_level)&4) (void)fflush(stdout);
#else
	(void)calc_tl_like(tl,perm,1,work,si);
#endif
}

int get_tl_position(double *pos)
{
	int i,j,k,k1,k2,link;
	double z,z1,z2,map_length;
	
	if(!n_markers) {
		pos[0]=pos[1]=0;
		return -1;
	}
	for(i=0;i<=n_links;i++) prior[i]=0.0;
	z1=0.0;
	for(k=0;k<=sex_map;k++) {
		z=total_maplength[k];
		z1+=z;
		for(i=0;i<n_links;i++) {
			map_length=linkage[i].r2[k]-linkage[i].r1[k];
			z-=map_length;
			prior[i]+=map_length;
		}
	}
	prior[i]+=z;
	do {
		z2=z1*safe_ranf();
		z=0.0;
		for(i=0;i<=n_links;i++) {
			if(prior[i]>0.0) {
				z+=prior[i];
				if(z2<=z) {
					z1=z2+prior[i]-z;
					break;
				}
			}
		}
	} while(i==n_links+1);
	link=(i==n_links?-1:i);
	if(link<0) pos[0]=pos[1]=0.0;
	else if(sex_map) {
		/* Get list of loci on linkage group */
		get_locuslist(perm,link,&k,0);
		gnu_qsort(perm,(size_t)k,(size_t)sizeof(int),cmp_loci_pos);
		/* Find interval */
		for(j=0;j<k;j++) {
			k1=perm[j];
			if(k1<0) {
				z=tlocus[-1-k1].locus.pos[0]-linkage[link].r1[0];
				z+=tlocus[-1-k1].locus.pos[1]-linkage[link].r1[1];
			} else {
				z=marker[k1].locus.pos[0]-linkage[link].r1[0];
				z+=marker[k1].locus.pos[1]-linkage[link].r1[1];
			}
			if(z>=z1) break;
		}
		/* Sample male and female positions (independently) within chosen interval */
		for(k2=0;k2<2;k2++) {
			if(j<k) {
				k1=perm[j];
				if(k1<0) z=tlocus[-1-k1].locus.pos[k2];
				else z=marker[k1].locus.pos[k2];
			} else z=linkage[link].r2[k2];
			if(j) {
				k1=perm[j-1];
				if(k1<0) z2=tlocus[-1-k1].locus.pos[k2];
				else z2=marker[k1].locus.pos[k2];
			} else z2=linkage[link].r1[k2];
			pos[k2]=z2+(z-z2)*safe_ranf();
		}
	} else pos[0]=pos[1]=z1+linkage[link].r1[0];
	return link;
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "TL_Birth_Death"
/* Trait locus Birth/Death */
void TL_Birth_Death(struct peel_mem *work,int si)
{
	int i,j,x=-1,step,nq,er=0;
	double l,l1,r,z,z1,u1,u2,u3,d,a,q1;
	double va_prop,vd_prop,alpha= -1,pp[2],old_res_var;
	double newpos[2],(*res_fn)(void);
	double old_res,mean_eff;
	int newlink,cens_flag,mtype;
	
#ifdef DEBUG
	double old_l;
#endif
	
	/* Count how many QTL currently in models[0] (nq) and no. unused QTL (j) */
	for(j=nq=x=0;x<n_tloci;x++) if(!tlocus[x].locus.flag) j++; else nq++;
	j+=max_tloci-n_tloci;
	if(!(j || nq)) return;
	pp[0]=j?BIRTH_STEP:0.0; /* Birth step possible if there is space for new QTL */
	pp[1]=nq>min_tloci?DEATH_STEP:0.0; /* Death step possible if no. QTL not at minimum */
	for(z=0.0,i=0;i<2;i++) z+=pp[i];
	/* We have a fixed no. loci - can't change anything */
	if(z<=0.0) return;
	step=(ranf()*z<=pp[0])?0:1;
	r=pp[step]/z; /* Proposal probability for step */
	if(!step) { /* Get position for new TL */
		newlink=get_tl_position(newpos);
	} else {
		newlink=0;
		newpos[0]=newpos[1]=0.0;
	}
	va_prop=residual_var[0]*PROP_RATIO;
	vd_prop=residual_var[0]*PROP_RATIO;
	/* Find proposal distribution for next time if current step was to be accepted */
	if(step) { /* Death steps reduce QTL numbers by 1 */
		pp[0]=BIRTH_STEP;
		pp[1]=((nq-1)>min_tloci)?DEATH_STEP:0.0;
	} else { /* Birth steps increase QTL numbers by 1 */
		pp[0]=(j-1)?BIRTH_STEP:0.0;
		pp[1]=DEATH_STEP;
	}
	for(z=0.0,i=0;i<2;i++) z+=pp[i];
#ifdef DEBUG
	if((*debug_level)&4) (void)printf("[%d]",step);
#endif
	move_stats[step].n++;
	mtype=models[0].var.type;
	if(!censor_mode && (mtype&ST_CENSORED)) {
		cens_flag=1;
		res_fn=&Calc_CensResLike;
	} else {
		cens_flag=0;
		res_fn=&Calc_ResLike;
	}
	old_res=res_fn();
#ifdef DEBUG
	if(old_res== -DBL_MAX) ABT_FUNC("Internal error - zero probability at start of Birth/Death step\n");
#endif
	/* If censoring and integrating over censored values, the sampled censored
	 * values will be invalid here, and will need to be sampled */
	if(cens_flag) Sample_Censored();
	old_res_var=residual_var[0];
	/* If censoring and integrating over censored values, the sampled censored
	 * values will be invalid here, and will need to be sampled */
	if(cens_flag) Sample_Censored();
	if(!step) { /* Birth Step */
		r=pp[1]/(z*r); /* Proposal ratio q(death)/q(birth) */
 		/* Factor in Poisson prior on number of loci */
		if(tloci_mean_set) r*=tloci_mean/(double)(1+nq);
		u1=genexp(va_prop);
		u2=genexp(vd_prop);
		for(u3=0.0,i=0;i<n_genetic_groups;i++) u3+=(new_freq[i]=ranf());
		u3/=(double)n_genetic_groups;
		d=sqrt(u2)/(2.0*u3*(1.0-u3));
		if(ranf()<0.5) d= -d;
		z=(ranf()<0.5)?-1.0:1.0;
		a=z*sqrt(u1/(2.0*u3*(1.0-u3)))-d*(1.0-2.0*u3);
		if(no_overdominant &&fabs(d)>fabs(a)) {
#ifdef DEBUG
			if((*debug_level)&4) {
				(void)fputc('f',stdout);
				(void)fflush(stdout);
			}
#endif
			return;
		}
		q1=Calc_Resprop();
		/* Find next available unused QTL */
		x=get_new_traitlocus(2);
		tlocus[x].model_flag=1;
		tlocus[x].locus.pos[0]=newpos[0];
		tlocus[x].locus.pos[1]=newpos[1];
		tlocus[x].locus.link_group=newlink;
		tlocus[x].locus.flag=(newlink<0?TL_UNLINKED:TL_LINKED);
		/* Get likelihood for QTL not in models[0] */
		l=old_res;
		for(i=0;i<n_genetic_groups;i++) {
			tlocus[x].locus.freq[i][0]=new_freq[i];
			tlocus[x].locus.freq[i][1]=1.0-new_freq[i];
		}
		tlocus[x].eff[0][0]=a+d;
		tlocus[x].eff[0][1]=a*2.0;
		mean_eff=(1.0-u3)*(1.0-u3)*tlocus[x].eff[0][1]+2.0*u3*(1.0-u3)*tlocus[x].eff[0][0];
		Adjust_Mean(mean_eff);
		/* Calculate likelihood (and sample) for models[0] with QTL (and mu = mu - mean_eff) */
		l1=calc_tl_like(x,perm,2,work,si);
		if(l1== -DBL_MAX) er=1;
		else {
			/* We'll need to sample more censored values conditional on the new QTL */
			if(cens_flag) Sample_Censored();
			z=Sample_ResVar();
			q1=q1-z+Calc_Res_Ratio(residual_var[0],old_res_var)+Calc_Var_Prior(residual_var[0])-Calc_Var_Prior(old_res_var);
			/* prior prob. for TL effects */
			z1=0.0;
			for(i=0;i<2;i++) z1+=tlocus[x].eff[0][i]*tlocus[x].eff[0][i];
			z=-.5*(2.0*log(2.0*M_PI*tau[0])+z1/tau[0]);
			/* proposal prob. for u1,u2 */
			z1= -u1/va_prop-u2/vd_prop-log(va_prop*vd_prop);
			/* det(jacobian) for transformation from (u1,u2,u3)->(effect[0],effect[1],p) */
			z1+=.5*(log(2.0*u1*u2)+3.0*(log(u3)+log(1.0-u3)));
			/* Acceptance ratio */
			alpha=safe_exp(l1+z-l-z1+q1);
#ifdef DEBUG
			if((*debug_level)&8) {(void)printf("<%g,%g,%g,%g,%g>",l,l1,z,z1,q1);}
#endif
			if(alpha<DBL_MAX) alpha*=r;
		}
		if(!er) z=ranf();
#ifdef DEBUG
		if((*debug_level)&8) {
			if(er) (void)fputs("(er)",stdout);
			else (void)printf("(%g,%g)",alpha,z);
		}
#endif
		if(!er && z<alpha)	{
			/* If successful, sample genotypes for new QTL */
#ifdef DEBUG
			if((*debug_level)&4) (void)fputc('S',stdout);
#endif
			move_stats[step].success++;
		} else {
			residual_var[0]=old_res_var;
			Adjust_for_TL(x,1.0);
			if(tlocus[x].locus.flag&LOCUS_SAMPLED) delete_traitlocus(x);
			tlocus[x].locus.flag=0;
			Adjust_Mean(-mean_eff);
#ifdef DEBUG		
			if((*debug_level)&1)	{
				l=res_fn();
				z=fabs(l-old_res);
				if(z>1.0e-8) {
					(void)printf("Birth: %g %g %g %d\n",old_res,l,z,er);
					ABT_FUNC("aborting\n");
				}
			}
			if((*debug_level)&4) (void)fputc('F',stdout);
#endif
		}
	} else if(step==1) { /* Death_Step */
		r=pp[0]/(z*r); /* Proposal ratio q(birth)/q(death) */
 		/* Factor in Poisson prior on number of loci */
		if(tloci_mean_set) r*=(double)nq/tloci_mean;
		/* Pick a non-blank QTL at random */
		i=(int)(safe_ranf()*(double)nq);
		for(x=0;x<n_tloci;x++) if(tlocus[x].locus.flag) if(!(i--)) break;
#ifdef DEBUG
		if((*debug_level)&1)	{
			old_l=calc_tl_like(x,perm,0,work,si);
			if(old_l== -DBL_MAX) ABT_FUNC("Internal error - zero probability at start of Death Step\n");
		} else old_l=0.0;
#endif
		q1=Calc_Resprop();
		for(u3=0.0,i=0;i<n_genetic_groups;i++) u3+=tlocus[x].locus.freq[i][0];
		u3/=(double)n_genetic_groups;
		mean_eff=(1.0-u3)*(1.0-u3)*tlocus[x].eff[0][1]+2.0*u3*(1.0-u3)*tlocus[x].eff[0][0];
		/* Adjust residuals as if taking QTL out of models[0] */
		Adjust_Mean(-mean_eff);
		Adjust_for_TL(x,1.0);
		/* Get new variance estimate */
		q1-=Sample_ResVar();
		/* Put QTL back */
		Adjust_Mean(mean_eff);
		Adjust_for_TL(x,-1.0);
		/* And calculate likelihood with new variance estimate */
		l=calc_tl_like(x,perm,0,work,si);
		q1+=Calc_Res_Ratio(residual_var[0],old_res_var)+Calc_Var_Prior(residual_var[0])-Calc_Var_Prior(old_res_var);
		/* and take means out again! */
		Adjust_Mean(-mean_eff);
		Adjust_for_TL(x,1.0);
		a=tlocus[x].eff[0][1]*.5;
		d=tlocus[x].eff[0][0]-a;
		z=a+d*(1.0-2.0*u3);
		u1=2.0*u3*(1.0-u3)*z*z;
		z=2.0*u3*(1.0-u3)*d;
		u2=z*z;
		/* Calculate likelihood if QTL is out of models[0] */
		l1=res_fn();
		if(l1== -DBL_MAX) alpha= -1.0;
		else {
			/* Prior for QTL effects */
			z1=0.0;
			for(i=0;i<2;i++) z1+=tlocus[x].eff[0][i]*tlocus[x].eff[0][i];
			z=-.5*(2.0*log(2.0*M_PI*tau[0])+z1/tau[0]);
			/* proposal prob. for u1,u2 */
			z1= -u1/va_prop-u2/vd_prop-log(va_prop*vd_prop);
			/* det(jacobian) for transformation from (u1,u2,u3)->(effect[0],effect[1],p) */
			z1+=.5*(log(2.0*u1*u2)+3.0*(log(u3)+log(1.0-u3)));
			/* Acceptance ratio */
			alpha=safe_exp(l1-l-z+z1+q1);
			if(alpha<DBL_MAX) alpha*=r;
		}
		z=ranf();
#ifdef DEBUG
		if((*debug_level)&8) (void)printf("(%g,%g)",alpha,z);
#endif
		if(z<alpha) {
			delete_traitlocus(x);
#ifdef DEBUG
			if((*debug_level)&4) (void)fputc('S',stdout);
#endif
			move_stats[step].success++;
		} else {
			residual_var[0]=old_res_var;
			Adjust_Mean(mean_eff);
			Adjust_for_TL(x,-1.0);
#ifdef DEBUG
			if((*debug_level)&1)	{
				l=res_fn();
				z=fabs(l-old_res);
				if(z>1.0e-8) {
					(void)printf("Death 1: %g %g %g %d\n",old_res,l,z,er);
					ABT_FUNC("aborting\n");
				}
				l=calc_tl_like(x,perm,0,work,si);
				z=fabs(l-old_l);
				if(z>1.0e-8) {
					(void)printf("Death 2: %g %g %g %d\n",old_l,l,z,er);
					ABT_FUNC("aborting\n");
				}
			}
			if((*debug_level)&4) (void)fputc('F',stdout);
#endif
		}
	}
}
