#include <stdlib.h>
#ifdef USE_DMALLOC
#include <dmalloc.h>
#endif
#include <math.h>
#include <stdio.h>

#include "ranlib.h"
#include "utils.h"
#include "loki.h"
#include "loki_peel.h"
#include "get_par_probs.h"
#include "loki_simple_peel.h"

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "loki_simple_sample"
/* Sample a nuclear family */
double loki_simple_sample(const struct Simple_Element *element,const int locus,pen_func pen,
								  lk_ulong **a_set,double **freq,struct R_Func *rf,struct peel_mem *work)
{
	int ids,idd,i,j,i1,j1,k,l,l1,m,n,fsp=0,n_off,*off,kid,*lump,*off_flag,no=0,no1=0,nb1,nmc,no2=0;
	int n_all,n_idx,comp,n_bits,*peel_pt2,*id_set1,*id_set2;
	double *tp,p1,p2,z,z1,prob=0.0,pp[4],*tmp,*tpp1,*tpp2;
	double *qval,*pval,*mval,*id_set;
	lk_ulong a,a1,b,b1,cm[2],*cm1[2],cmp,cmm,*t_cm,*t_all1,*t_all2;
	lk_ulong *tt_all,*peel_pt1;
	struct fset *peel_fs,*t_fset;

	ids=element->sire-1;
	idd=element->dam-1;
	comp=id_array[ids].comp;
	n_all=marker[locus].n_all1[comp];
	n_bits=num_bits(n_all);
	n_idx=1<<(n_bits+n_bits);
	off=element->off;
	n_off=element->n_off;
	nb1=1<<n_bits;
	k=n_all*n_all;
	peel_fs=work->s0;
	id_set1=work->s1;
	id_set2=id_set1+k;
	id_set=work->s2;
	qval=id_set+k;
	pval=qval+n_idx;
	mval=pval+n_idx;
	tt_all=work->s3;
	peel_pt1=tt_all+max_peel_off*n_all;
	peel_pt2=work->s4;
#ifdef TRACE_PEEL
	if(CHK_PEEL(TRACE_LEVEL_1)) (void)printf("In %s(%p,%d,%p)\n",FUNC_NAME,(void *)element,locus,(void *)pen);
	if(CHK_PEEL(TRACE_LEVEL_2)) {
		if(family_id) {
			print_orig_family(stdout,off[0]+1,0);
			(void)fputc(' ',stdout);
		}
		print_orig_id1(stdout,ids+1);
		(void)fputc(',',stdout);
		print_orig_id1(stdout,idd+1);
		(void)fputc(' ',stdout);
		for(i=0;i<n_off;i++) {
			(void)fputc(i?',':'(',stdout);
			print_orig_id1(stdout,off[i]);
		}
		(void)fputs(")\n",stdout);
	}
#endif
	cm1[0]=peel_pt1;
	cm1[1]=cm1[0]+n_off;
	off_flag=peel_pt2;
	p1=get_par_probs(pval,ids,locus,pen,a_set,freq,rf);
	prob+=log(p1);
	if(id_array[ids].flag&SAMPLED_MAT) {
#ifdef TRACE_PEEL
		if(CHK_PEEL(TRACE_LEVEL_3)) (void)fputs("Sire sampled\n",stdout);
#endif
	} else {
#ifdef TRACE_PEEL
		if(CHK_PEEL(TRACE_LEVEL_3)) (void)printf("Sire total (b): %g\n",p1);
#endif
		for(m=0;m<n_off;m++) {
			kid=off[m]-1;
			cmm=req_set[X_PAT][locus][kid];
			if(id_array[kid].flag&SAMPLED_MAT) cm1[X_PAT][m]=LK_ONE<<(id_array[kid].allele[X_PAT]-1);
			else cm1[X_PAT][m]=id_array[kid].temp[X_PAT];
			if(cmm&cm1[X_PAT][m]) cm1[X_PAT][m]|=cmm;
			off_flag[m]=1;
		}
		if(n_off>1) {
			for(i=1;i<n_off;i++)	{	
				a=cm1[X_PAT][i];
				for(j=0;j<i;j++) {
					b=cm1[X_PAT][j];
					if((a&b)==a) off_flag[j]=0;
					else if((a&b)==b) off_flag[i]=0;
				}
			}
			no1=0;
			for(i=0;i<n_off;i++) if(off_flag[i]) cm1[X_PAT][no1++]=cm1[X_PAT][i];
		} else no1=n_off;
	}
	p1=get_par_probs(mval,idd,locus,pen,a_set,freq,rf);
	prob+=log(p1);
	if(id_array[idd].flag&SAMPLED_MAT) {
#ifdef TRACE_PEEL
		if(CHK_PEEL(TRACE_LEVEL_3)) (void)fputs("Dam sampled\n",stdout);
#endif
	} else {
#ifdef TRACE_PEEL
		if(CHK_PEEL(TRACE_LEVEL_3)) (void)printf("Dam total (b): %g\n",p1);
#endif
		for(m=0;m<n_off;m++) {
			kid=off[m]-1;
 			cmm=req_set[X_MAT][locus][kid];
			if(id_array[kid].flag&SAMPLED_MAT) cm1[X_MAT][m]=LK_ONE<<(id_array[kid].allele[X_MAT]-1);
			else cm1[X_MAT][m]=id_array[kid].temp[X_MAT];
			if(cmm&cm1[X_MAT][m]) cm1[X_MAT][m]|=cmm;
			off_flag[m]=1;
		}
		if(n_off>1) {
			for(i=1;i<n_off;i++)	{	
				a=cm1[X_MAT][i];
				for(j=0;j<i;j++) {
					b=cm1[X_MAT][j];
					if((a&b)==a) off_flag[j]=0;
					else if((a&b)==b) off_flag[i]=0;
				}
			}
			no=0;
			for(i=0;i<n_off;i++) if(off_flag[i]) cm1[X_MAT][no++]=cm1[X_MAT][i];
		} else no=n_off;
	}
	if(!((id_array[ids].flag&SAMPLED_MAT)&&(id_array[idd].flag&SAMPLED_MAT))) {
		t_all1=tt_all;
		for(m=0;m<n_off;m++) {
			kid=off[m]-1;
			if(id_array[kid].flag&SAMPLED_MAT) continue;
			cm[X_MAT]=req_set[X_MAT][locus][kid];
			cm[X_PAT]=req_set[X_PAT][locus][kid];
			for(i=0;i<n_all;i++) t_all1[i]=0;
			for(a1=LK_ONE,i=0;i<n_all;i++,a1<<=1) {
				a=a_set[kid][i];
				if(a) {
					if(a&cm[X_PAT]) a|=cm[X_PAT];
					if(a1&cm[X_MAT]) {
						b=cm[X_MAT];
						j=0;
						while(b) {
							if(b&1) t_all1[j]=a;
							j++;
							b>>=1;
						}
					} else t_all1[i]=a;
				}
			}
			t_all2=tt_all;
			for(k=0;k<no2;k++) {
				for(i=0;i<n_all;i++) if(t_all1[i]!=t_all2[i]) break;
				if(i==n_all) break;
				t_all2+=n_all;
			} 
			if(k==no2) {
				no2++;
				t_all1+=n_all;
			} 
		}
	} 
	t_fset=peel_fs;
	if(id_array[ids].flag&SAMPLED_MAT) {
		if(id_array[idd].flag&SAMPLED_MAT) {
			t_fset->pat_gene[X_MAT]=id_array[ids].allele[X_MAT]-1;
			t_fset->pat_gene[X_PAT]=id_array[ids].allele[X_PAT]-1;
			t_fset->mat_gene[X_MAT]=id_array[idd].allele[X_MAT]-1;
			t_fset->mat_gene[X_PAT]=id_array[idd].allele[X_PAT]-1;
			t_fset->p=1.0;
			fsp=1;
		} else {
			i=id_array[ids].allele[X_MAT]-1;
			j=id_array[ids].allele[X_PAT]-1;
			cmp=(LK_ONE<<i)|(LK_ONE<<j);
			for(k=0;k<n_all;k++) {
				b=a_set[idd][k];
				l=0;
				l1=k;
				while(b) {
					if(b&1) {
						cmm=(LK_ONE<<k)|(LK_ONE<<l);
						t_cm=cm1[X_MAT];
						for(m=0;m<no;m++) if(!(*(t_cm++)&cmm)) break;
						if(m==no) {
							t_all1=tt_all;
							for(m=0;m<no2;m++,t_all1+=n_all) if(!((t_all1[k]&cmp)||(t_all1[l]&cmp))) break;
							if(m==no2) {
								t_fset->pat_gene[X_MAT]=i;
								t_fset->pat_gene[X_PAT]=j;
								t_fset->mat_gene[X_MAT]=k;
								t_fset->mat_gene[X_PAT]=l;
								(t_fset++)->p=mval[l1];
								fsp++;
							}
						}
					}
					b>>=1;
					l++;
					l1+=nb1;
				}
			}
		}
	} else if(id_array[idd].flag&SAMPLED_MAT) {
		k=id_array[idd].allele[X_MAT]-1;
		l=id_array[idd].allele[X_PAT]-1;
		for(i=0;i<n_all;i++) {
			a=a_set[ids][i];
			j=0;
			l1=i;
			while(a) {
				if(a&1) {
					cmp=(LK_ONE<<i)|(LK_ONE<<j);
					t_cm=cm1[X_PAT];
					for(m=0;m<no1;m++) if(!(*(t_cm++)&cmp)) break;
					if(m==no1) {
						t_all1=tt_all;
						for(m=0;m<no2;m++,t_all1+=n_all) if(!((t_all1[k]&cmp)||(t_all1[l]&cmp))) break;
						if(m==no2) {
							t_fset->pat_gene[X_MAT]=i;
							t_fset->pat_gene[X_PAT]=j;
							t_fset->mat_gene[X_MAT]=k;
							t_fset->mat_gene[X_PAT]=l;
							(t_fset++)->p=pval[l1];
							fsp++;
						}
					}
				}
				a>>=1;
				j++;
				l1+=nb1;
			}
		}
	} else {
		for(nmc=k=0;k<n_all;k++) {
			b=a_set[idd][k];
			l=0;
			l1=k;
			while(b) {
				if(b&1) {
				cmm=(LK_ONE<<k)|(LK_ONE<<l);
					t_cm=cm1[X_MAT];
					for(m=0;m<no;m++) if(!(*(t_cm++)&cmm)) break;
					if(m==no) {
						id_set[nmc]=mval[l1];
						id_set1[nmc]=k;
						id_set2[nmc++]=l;
					}
				}
				b>>=1;
				l++;
				l1+=nb1;
			}
		}
		switch(no2) {
		 case 1:
			for(a1=LK_ONE,i=0;i<n_all;i++,a1<<=1) {
				a=a_set[ids][i];
				j=0;
				b1=LK_ONE;
				while(a) {
					if(a&1) {
						cmp=a1|b1;
						t_cm=cm1[X_PAT];
						for(m=0;m<no1;m++) if(!(*(t_cm++)&cmp)) break;
						if(m==no1) {
							z=pval[(j<<n_bits)|i];
							for(l1=0;l1<nmc;l1++) {
								k=id_set1[l1];
								l=id_set2[l1];
								if((tt_all[k]&cmp)||(tt_all[l]&cmp)) {
									t_fset->pat_gene[X_MAT]=i;
									t_fset->pat_gene[X_PAT]=j;
									t_fset->mat_gene[X_MAT]=k;
									t_fset->mat_gene[X_PAT]=l;
									(t_fset++)->p=z*id_set[l1];
									fsp++;
								} 
							}
						}
					}
					a>>=1;
					j++;
					b1<<=1;
				}
			}
			break;
		 case 2:
			t_all1=tt_all+n_all;
			for(a1=LK_ONE,i=0;i<n_all;i++,a1<<=1) {
				a=a_set[ids][i];
				j=0;
				b1=LK_ONE;
				while(a) {
					if(a&1) {
						cmp=a1|b1;
						t_cm=cm1[X_PAT];
						for(m=0;m<no1;m++) if(!(*(t_cm++)&cmp)) break;
						if(m==no1) {
							z=pval[(j<<n_bits)|i];
							for(l1=0;l1<nmc;l1++) {
								k=id_set1[l1];
								l=id_set2[l1];
								if(((tt_all[k]&cmp)||(tt_all[l]&cmp))&&((t_all1[k]&cmp)||(t_all1[l]&cmp))) {
									t_fset->pat_gene[X_MAT]=i;
									t_fset->pat_gene[X_PAT]=j;
									t_fset->mat_gene[X_MAT]=k;
									t_fset->mat_gene[X_PAT]=l;
									(t_fset++)->p=z*id_set[l1];
									fsp++;
								} 
							}
						}
					}
					a>>=1;
					j++;
					b1<<=1;
				}
			}
			break;
		 case 0:
			for(a1=LK_ONE,i=0;i<n_all;i++,a1<<=1) {
				a=a_set[ids][i];
				j=0;
				b1=LK_ONE;
				while(a) {
					if(a&1) {
						cmp=a1|b1;
						t_cm=cm1[X_PAT];
						for(m=0;m<no1;m++) if(!(*(t_cm++)&cmp)) break;
						if(m==no1) {
							z=pval[(j<<n_bits)|i];
							for(l1=0;l1<nmc;l1++) {
								t_fset->pat_gene[X_MAT]=i;
								t_fset->pat_gene[X_PAT]=j;
								t_fset->mat_gene[X_MAT]=id_set1[l1];
								t_fset->mat_gene[X_PAT]=id_set2[l1];
								(t_fset++)->p=z*id_set[l1];
								fsp++;
							}
						}
					}
					a>>=1;
					j++;
					b1<<=1;
				}
			}
			break;
		 default:
			for(a1=LK_ONE,i=0;i<n_all;i++,a1<<=1) {
				a=a_set[ids][i];
				j=0;
				b1=LK_ONE;
				while(a) {
					if(a&1) {
						cmp=a1|b1;
						t_cm=cm1[X_PAT];
						for(m=0;m<no1;m++) if(!(*(t_cm++)&cmp)) break;
						if(m==no1) {
							z=pval[(j<<n_bits)|i];
							for(l1=0;l1<nmc;l1++) {
								k=id_set1[l1];
								l=id_set2[l1];
								t_all1=tt_all;
								for(m=0;m<no2;m++,t_all1+=n_all) if(!((t_all1[k]&cmp)||(t_all1[l]&cmp))) break;
								if(m==no2) {
									t_fset->pat_gene[X_MAT]=i;
									t_fset->pat_gene[X_PAT]=j;
									t_fset->mat_gene[X_MAT]=k;
									t_fset->mat_gene[X_PAT]=l;
									(t_fset++)->p=z*id_set[l1];
									fsp++;
								} 
							}
						}
					}
					a>>=1;
					j++;
					b1<<=1;
				}
			}
		}
	}
#ifdef DEBUG
	if(!fsp)	{
		ABT_FUNC(" - Internal error - no parental combinations\n");
	}
	for(k=0;k<fsp;k++) if(isnan(peel_fs[k].p)) {
		ABT_FUNC("Floating point error\n");
	}
#endif
	for(m=0;m<n_off;m++) {
		kid=off[m]-1;
		tp=id_array[kid].tp;
		cm[0]=req_set[0][locus][kid];
		cm[1]=req_set[1][locus][kid];
		t_fset=peel_fs;
		if(id_array[kid].flag&SAMPLED_MAT) {
			j=id_array[kid].allele[X_MAT]-1;
			k=id_array[kid].allele[X_PAT]-1;
			l1=(k<<n_bits)|j;
			if(!(cm[0] || cm[1])) for(n=0;n<fsp;n++) {
				i1=(t_fset->pat_gene[X_MAT]<<n_bits);
				j1=(t_fset->pat_gene[X_PAT]<<n_bits);
				k=peel_fs[n].mat_gene[X_MAT];
				l=peel_fs[n].mat_gene[X_PAT];
				z=0.0;
				if((i1|k)==l1) z+=tp[X_MM_PM];
				if((j1|k)==l1) z+=tp[X_MM_PP];
				if((i1|l)==l1) z+=tp[X_MP_PM];
				if((j1|l)==l1) z+=tp[X_MP_PP];
				(t_fset++)->p*=z;
			} else {
				lump=id_array[kid].lumped;
				l=lump[X_PAT]<<n_bits;
				j1=1<<n_bits;
				for(i=i1=0,a1=LK_ONE;i<n_all;i++,a1<<=1,i1+=j1) {
					id_set1[i]=(cm[X_PAT]&a1)?l:i1;
					id_set2[i]=(cm[X_MAT]&a1)?lump[X_MAT]:i;
				}
				for(n=0;n<fsp;n++) {
					i1=id_set1[t_fset->pat_gene[X_MAT]];
					j1=id_set1[t_fset->pat_gene[X_PAT]];
					k=id_set2[t_fset->mat_gene[X_MAT]];
					l=id_set2[t_fset->mat_gene[X_PAT]];
					z=0.0;
					if((i1|k)==l1) z+=tp[X_MM_PM];
					if((j1|k)==l1) z+=tp[X_MM_PP];
					if((i1|l)==l1) z+=tp[X_MP_PM];
					if((j1|l)==l1) z+=tp[X_MP_PP];
					(t_fset++)->p*=z;
				}
			}
		} else {
			if(!pen) {
				if((k=id_array[kid].rfp)>=0) {/* Insert Previously computed R_Func */
					for(i1=0;i1<n_all;i1++) {
						tmp=qval+(i1<<n_bits);
						for(l=0;l<n_all;l++) *(tmp++)=0.0;
					}
					i1=rf[k].n_terms;
					tmp=rf[k].p;
					for(j=0;j<i1;j++) qval[rf[k].index[j]]=*(tmp++);
				} else {
					for(j=0;j<n_all;j++) {
						a=a_set[kid][j];
						tmp=qval+j;
						k=0;
						while(a) {
							*tmp=(a&1)?1.0:0.0;
							a>>=1;
							tmp+=nb1;
							k++;
						}
						for(;k<n_all;k++) { 
							*tmp=0.0;
							tmp+=nb1;
						}
					}
				}
			} else {
				tmp=qval;
				for(j=0;j<n_idx;j++) *(tmp++)=0.0;
				if((k=id_array[kid].rfp)>=0) { /* Insert Previously computed R_Func */
					i1=rf[k].n_terms;
					tmp=rf[k].p;
					for(j=0;j<i1;j++) qval[rf[k].index[j]]=*(tmp++);
				} else {
					for(j=0;j<n_all;j++) {
						a=a_set[kid][j];
						tmp=qval+j;
						while(a) {
							if(a&1) *tmp=1.0;
							a>>=1;
							tmp+=nb1;
						}
					}
				}
				pen(qval,kid,locus,n_all,n_bits);
			}
			if(!(cm[0] || cm[1])) for(n=0;n<fsp;n++) {
				i1=(t_fset->pat_gene[X_MAT])<<n_bits;
				j1=(t_fset->pat_gene[X_PAT])<<n_bits;
				k=t_fset->mat_gene[X_MAT];
				l=t_fset->mat_gene[X_PAT];
				z=tp[X_MM_PM]*qval[i1|k]+tp[X_MM_PP]*qval[j1|k]+tp[X_MP_PM]*qval[i1|l]+tp[X_MP_PP]*qval[j1|l];
				(t_fset++)->p*=z;
			} else {
				lump=id_array[kid].lumped;
				l=lump[X_PAT]<<n_bits;
				j1=1<<n_bits;
				for(i=i1=0,a1=LK_ONE;i<n_all;i++,a1<<=1,i1+=j1) {
					id_set1[i]=(cm[X_PAT]&a1)?l:i1;
					id_set2[i]=(cm[X_MAT]&a1)?lump[X_MAT]:i;
				}
				tpp1=id_array[kid].tpp[X_PAT];
				tpp2=id_array[kid].tpp[X_MAT];
				for(n=0;n<fsp;n++) {
					i1=id_set1[t_fset->pat_gene[X_MAT]];
					j1=id_set1[t_fset->pat_gene[X_PAT]];
					k=id_set2[t_fset->mat_gene[X_MAT]];
					l=id_set2[t_fset->mat_gene[X_PAT]];
					z=tp[X_MM_PM]*qval[i1|k]+tp[X_MM_PP]*qval[j1|k]+tp[X_MP_PM]*qval[i1|l]+tp[X_MP_PP]*qval[j1|l];
					(t_fset++)->p*=z; 
				}
			}
		}
#ifdef DEBUG
		for(k=0;k<fsp;k++) if(isnan(peel_fs[k].p)) {
			ABT_FUNC("Floating point error\n");
		}
#endif
	}
	p1=0.0;
	for(n=0;n<fsp;n++) p1+=peel_fs[n].p;
#ifdef DEBUG
	if(!fsp || p1<=0.0 || isnan(p1)) {
		ABT_FUNC("Internal error - zero probability for parents\n");
	}
#endif
	prob+=log(p1);
	if(!((id_array[ids].flag&SAMPLED_MAT)&&(id_array[idd].flag&SAMPLED_MAT))) {
		do {
			z=ranf()*p1;
			p2=0.0;
			t_fset=peel_fs;
			for(n=0;n<fsp;n++,t_fset++) {
				p2+=t_fset->p;
				if(p2>=z) {
					id_array[ids].allele[X_MAT]=t_fset->pat_gene[X_MAT]+1;
					id_array[ids].allele[X_PAT]=t_fset->pat_gene[X_PAT]+1;
					id_array[idd].allele[X_MAT]=t_fset->mat_gene[X_MAT]+1;
					id_array[idd].allele[X_PAT]=t_fset->mat_gene[X_PAT]+1;
					id_array[ids].flag|=(SAMPLED_MAT|SAMPLED_PAT);
					id_array[idd].flag|=(SAMPLED_MAT|SAMPLED_PAT);
#ifdef DEBUG
					if(id_array[ids].allele[X_MAT]<1 || id_array[ids].allele[X_MAT]>n_all) {
						ABT_FUNC("Bad sample allele\n");
					}
					if(id_array[ids].allele[X_PAT]<1 || id_array[ids].allele[X_PAT]>n_all) {
						ABT_FUNC("Bad sample allele\n");
					}
					if(id_array[idd].allele[X_MAT]<1 || id_array[idd].allele[X_MAT]>n_all) {
						ABT_FUNC("Bad sample allele\n");
					}
					if(id_array[idd].allele[X_MAT]<1 || id_array[idd].allele[X_PAT]>n_all) {
						ABT_FUNC("Bad sample allele\n");
					}
#endif
					break;
				}
			}
		} while(n==fsp);
	}
	for(m=0;m<n_off;m++) {
		kid=off[m]-1;
		if(id_array[kid].flag&SAMPLED_MAT) continue;
		if(!pen) {
			if((k=id_array[kid].rfp)>=0) {/* Insert Previously computed R_Func */
				for(i1=0;i1<n_all;i1++) {
					tmp=qval+(i1<<n_bits);
					for(l=0;l<n_all;l++) *(tmp++)=0.0;
				}
				i1=rf[k].n_terms;
				tmp=rf[k].p;
				for(j=0;j<i1;j++) qval[rf[k].index[j]]=*(tmp++); 
			} else {
				for(j=0;j<n_all;j++) {
					a=a_set[kid][j];
					tmp=qval+j;
					k=0;
					while(a) {
						*tmp=(a&1)?1.0:0.0;
						a>>=1;
						tmp+=nb1;
						k++;
					}
					for(;k<n_all;k++) {
						*tmp=0.0;
						tmp+=nb1;
					} 
				}
			}
		} else {
			tmp=qval;
			for(j=0;j<n_idx;j++) *(tmp++)=0.0;
			if((k=id_array[kid].rfp)>=0) {/* Insert Previously computed R_Func */
				i1=rf[k].n_terms;
				tmp=rf[k].p;
				for(j=0;j<i1;j++) qval[rf[k].index[j]]=*(tmp++); 
			} else {
				for(j=0;j<n_all;j++) {
					a=a_set[kid][j];
					tmp=qval+j;
					while(a) {
						if(a&1) *tmp=1.0;
						a>>=1;
						tmp+=nb1;
					}
				}
			}
			pen(qval,kid,locus,n_all,n_bits);
		}
		/* transmission probs */
		tp=id_array[kid].tp;
		cm[0]=req_set[0][locus][kid];
		cm[1]=req_set[1][locus][kid];
		i=id_array[ids].allele[X_MAT]-1;
		j=id_array[ids].allele[X_PAT]-1;
		k=id_array[idd].allele[X_MAT]-1;
		l=id_array[idd].allele[X_PAT]-1;
		if((LK_ONE<<i)&cm[X_PAT]) i=id_array[kid].lumped[X_PAT];
		if((LK_ONE<<j)&cm[X_PAT]) j=id_array[kid].lumped[X_PAT];
		if((LK_ONE<<k)&cm[X_MAT]) k=id_array[kid].lumped[X_MAT];
		if((LK_ONE<<l)&cm[X_MAT]) l=id_array[kid].lumped[X_MAT];
		i1=i<<n_bits;
		j1=j<<n_bits;
		p1=(pp[X_MM_PM]=tp[X_MM_PM]*qval[i1|k]);
		p1+=(pp[X_MM_PP]=tp[X_MM_PP]*qval[j1|k]);
		p1+=(pp[X_MP_PM]=tp[X_MP_PM]*qval[i1|l]);
		p1+=(pp[X_MP_PP]=tp[X_MP_PP]*qval[j1|l]);
		if(p1<=0.0) ABT_FUNC("Internal error - zero probability for child sample\n");
		do {
			z=ranf()*p1;
			p2=0.0;
			tmp=pp+3;
			n=4;
			while(n--) {
				z1=*(tmp--);
				if(z1>0.0) {
					p2+=z1;
					if(z<=p2) break;
				}
			}
		} while(n<0);
		switch(n) {
		 case X_MM_PM:
			id_array[kid].allele[X_MAT]=k+1;
			id_array[kid].allele[X_PAT]=i+1;
			break;
		 case X_MM_PP:
			id_array[kid].allele[X_MAT]=k+1;
			id_array[kid].allele[X_PAT]=j+1;
			break;
		 case X_MP_PM:
			id_array[kid].allele[X_MAT]=l+1;
			id_array[kid].allele[X_PAT]=i+1;
			break;
		 case X_MP_PP:
			id_array[kid].allele[X_MAT]=l+1;
			id_array[kid].allele[X_PAT]=j+1;
			break;
		 default:
			ABT_FUNC("Internal error - illegal sample\n");
		}
		id_array[kid].flag|=(SAMPLED_MAT|SAMPLED_PAT);
#ifdef DEBUG
		if(id_array[kid].allele[X_MAT]<1 || id_array[kid].allele[X_MAT]>n_all) {
			ABT_FUNC("Bad sampled allele\n");
		}
		if(id_array[kid].allele[X_PAT]<1 || id_array[kid].allele[X_PAT]>n_all) {
			ABT_FUNC("Bad sampled allele\n");
		}
#endif
	}
	k=element->out_index;
#ifdef TRACE_PEEL
	if(CHK_PEEL(TRACE_LEVEL_2)) {
		(void) printf("Returning from %s() with %g\n",FUNC_NAME,prob);
	}
#endif
	return prob;
}

