/****************************************************************************
 *                                                                          *
 *     Loki - Programs for genetic analysis of complex traits using MCMC    *
 *                                                                          *
 *             Simon Heath - University of Washington                       *
 *                         - Rockefeller University                         *
 *                                                                          *
 *                        August 1997                                       *
 *                                                                          *
 * loki_peel.c:                                                             *
 *                                                                          *
 * Perform peeling calculations                                             *
 *                                                                          *
 * 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 "get_par_probs.h"
#include "loki_simple_peel.h"
#include "loki_trait_simple_peel.h"

static double **freq;
static struct Peelseq_Head **peel_list;
static int *peel_alls;
struct peel_mem_block *first_mem_block[2],*mem_block[2];

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "peel_alloc"
void peel_alloc(struct peel_mem *work)
{
	int i,j,j1,k,k1,k2;
	
#ifdef TRACE_PEEL
	if(CHK_PEEL(TRACE_LEVEL_1)) (void)printf("In %s()\n",FUNC_NAME);
#endif
	
	k=tlocus?2:0;
	for(i=0;i<n_markers;i++) {
		j=marker[i].locus.n_alleles;
		if(j>k) k=j;
	}
	if(!k) return;
	if(!(freq=malloc(sizeof(void *)*2*n_genetic_groups))) ABT_FUNC(MMsg);
	if(!(freq[0]=malloc(sizeof(double)*k*2*n_genetic_groups))) ABT_FUNC(MMsg);
	for(i=1;i<n_genetic_groups*2;i++) freq[i]=freq[i-1]+k;
	k1=k*k;
	if(!(work->s0=malloc(sizeof(struct fset)*k1*k1))) ABT_FUNC(MMsg);
	if(!(work->s1=malloc(sizeof(int)*(k+k1*2)))) ABT_FUNC(MMsg);
	peel_alls=work->s1+2*k1;
	j=num_bits(k);
	j=1<<(j+j);
   /* Note the 16 below should change if more than diallelic trait loci are fitted */
	if(!(work->s2=malloc(sizeof(double)*(16+k1+4*j)))) ABT_FUNC(MMsg);
	if(!(work->s3=malloc(sizeof(lk_ulong)*max_peel_off*(k+2)))) ABT_FUNC(MMsg);
	if(!(work->s4=malloc(sizeof(int)*max_peel_off))) ABT_FUNC(MMsg);
	for(k2=i=0;i<n_comp;i++) {
		j1=comp_npeel[i];
		if(j1>k2) k2=j1;
	}
	if(!(work->s5=malloc(sizeof(void *)*k2))) ABT_FUNC(MMsg);
	j1=n_markers+max_tloci;
	if(!(work->s6=malloc(sizeof(void *)*j1))) ABT_FUNC(MMsg);
	if(!(work->s7=malloc(sizeof(double)*2*j1))) ABT_FUNC(MMsg);
	peel_list=(struct Peelseq_Head **)work->s5;
	first_mem_block[MRK_MBLOCK]=get_new_memblock(MB_SIZE,MRK_MBLOCK);
	first_mem_block[TRT_MBLOCK]=get_new_memblock(MB_SIZE,TRT_MBLOCK);
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "peel_dealloc"
void peel_dealloc(struct peel_mem *work)
{
	int k;
	struct peel_mem_block *p,*p1;
	
#ifdef TRACE_PEEL
	if(CHK_PEEL(TRACE_LEVEL_1)) (void)printf("In %s()\n",FUNC_NAME);
#endif
	if(freq) {
		if(freq[0]) free(freq[0]);
		free(freq);
	}
	if(work) {
		if(work->s0) free(work->s0);
		if(work->s1) free(work->s1);
		if(work->s2) free(work->s2);
		if(work->s3) free(work->s3);
		if(work->s4) free(work->s4);
		if(work->s5) free(work->s5);
		if(work->s6) free(work->s6);
		if(work->s7) free(work->s7);
	}
	for(k=0;k<2;k++) {
		p1=first_mem_block[k];
		while(p1) {
			p=p1->next;
			if(p1->index) free(p1->index);
			if(p1->val) free(p1->val);
			free(p1);
			p1=p;
		}
	}
}

#ifdef FUNC_NAME
#undef FUNC_NAME
#endif
#define FUNC_NAME "peel_locus"
/* Peels locus perm[idx] where the n_loci entries in perm are all in 1 linkage group.
 * If sample_flag then the locus is to be sampled */
double peel_locus(const int *perm,int idx,int n_loci,int sample_flag,struct peel_mem *work,int si)
{
	struct Peelseq_Head *pp;
	struct Simple_Element *simple_em;
   struct Complex_Element *complex_em; 
	struct Id_Record *id;
	struct Marker *mark=0;
	struct TraitLocus *tloc=0;
	trait_pen_func *trait_pen=0;
	int comp,i,i1,j,k,k1,k2,k3,nn_all,n_peel_ops,ids,idd,allele,n_rf,grp,cs,*a_trans;
	int rec,nrec,mtype,locus,sample_freq=0,trn[2],res_flag,locus1,**seg,unlinked=0,*f_flag;
	int ***seglist,n_all,linktype;
	double *recom1,*recom2;
   double like=0.0,like1,z,z1,theta,Mtp[2],Ptp[2],*pos,*eff,**count,*tpp,*freq1,*count1;
	signed char *freq_set;
	lk_ulong a,b,c,lump,**a_set=0;
	pen_func *pen=0;
	struct Locus *loc,*loc1;
	struct R_Func *rf;
	
#ifdef TRACE_PEEL
	if(CHK_PEEL(TRACE_LEVEL_1)) (void)printf("In %s(%p,%d,%d,%d)\n",FUNC_NAME,(void *)perm,idx,n_loci,sample_flag);
#endif
	seglist=(int ***)work->s6;
	recom1=work->s7;
	recom2=recom1+n_markers+n_tloci;
	locus=perm[idx];
	sample_flag&=~OP_SAMPLING;
	if(locus<0) { /* Trait locus */
		tloc=&tlocus[-1-locus];
		loc=&tloc->locus;
		locus1=n_markers;
		nn_all=loc->n_alleles;
		if(loc->flag&TL_LINKED) {
			j=loc->link_group;
			linktype=linkage[j].type;
		} else {
			linktype=LINK_AUTO;
			unlinked=1;
		}
		mtype=models[0].var.type;
		if(!multiple_rec && !use_student_t && (loc->flag&LOCUS_SAMPLED) && (!(mtype&ST_CENSORED) || censor_mode)) trait_pen=s_penetrance1;
		else trait_pen=s_penetrance;
		sample_freq=sample_flag==1?1:0;
	} else { /* Marker locus */
		locus1=locus;
		mark=marker+locus;
		loc=&mark->locus;
		nn_all=loc->n_alleles;
		a_set=all_set[locus];
		sample_freq=0;
		if(sample_flag==1) for(k=0;k<n_genetic_groups;k++) {
			for(i=j=0;i<nn_all;i++) if(mark->freq_set[k][i]!=1) j++;
			sample_freq|=(j>1)?1:0;
		}
		j=loc->link_group;
		linktype=linkage[j].type;
	}
	pos=loc->pos;
	seg=loc->seg;
#ifdef TRACE_PEEL
	if(CHK_PEEL(TRACE_LEVEL_2)) {
		(void)printf("locus %s, sample_flag=%d, nn_all=%d, linktype=%d\n",locus<0?"QTL":mark->name,sample_flag,nn_all,linktype);
	}
#endif
	if(nn_all<2) return 0.0;
	if(n_loci>1) {
		k2=0;
		for(i=0;i<idx;i++) {
			k1=perm[i];
			loc1=k1>=0?&marker[k1].locus:&tlocus[-1-k1].locus;
			seglist[k2]=loc1->seg;
			recom1[k2]=.5*(1.0-exp(-0.02*(pos[X_MAT]-loc1->pos[X_MAT])));
			recom2[k2++]=.5*(1.0-exp(-0.02*(pos[X_PAT]-loc1->pos[X_PAT])));
#ifdef DEBUG
			if(recom1[k2-1]<=0.0 || recom2[k2-1]<=0.0) {
				ABT_FUNC("Zero recomb - this will not work!\n");
			}
			if(recom1[k2-1]<=1.0e-16 || recom2[k2-1]<=1.0e-16) {
				fprintf(stderr,"Warning: [1] Very low recomb (%g,%g) - this may not work!\n",recom1[k2-1],recom2[k2-1]);
			}
#endif
		}
		for(i=idx+1;i<n_loci;i++) {
			k1=perm[i];
			loc1=k1>=0?&marker[k1].locus:&tlocus[-1-k1].locus;
			seglist[k2]=loc1->seg;
			recom1[k2]=.5*(1.0-exp(0.02*(pos[X_MAT]-loc1->pos[X_MAT])));
			recom2[k2++]=.5*(1.0-exp(0.02*(pos[X_PAT]-loc1->pos[X_PAT])));
#ifdef DEBUG
			if(recom1[k2-1]<=0.0 || recom2[k2-1]<=0.0) {
				ABT_FUNC("Zero recomb - this will not work!\n");
			}
			if(recom1[k2-1]<=1.0e-16 || recom2[k2-1]<=1.0e-16) {
				fprintf(stderr,"Warning: [2] Very low recomb (%g,%g) - this may not work!\n",recom1[k2-1],recom2[k2-1]);
			}
#endif
		}
	}
	count=freq+n_genetic_groups;
	if(sample_freq) {
		for(j=0;j<n_genetic_groups;j++) {
			if(locus>=0 && mark->count_flag[j]) for(i=0;i<nn_all;i++) count[j][i]=mark->counts[j][i]+1.0;
			else for(i=0;i<nn_all;i++) count[j][i]=1.0;
		}
	}
	if(locus<0)	{
		for(i=0;i<nn_all;i++) {
			peel_alls[i]=i;
			for(j=0;j<n_genetic_groups;j++) freq[j][i]=tloc->locus.freq[j][i];
		}
	}
	eff=0;
	mtype=0;
	res_flag=0;
	if(locus<0 || (mark->mterm && mark->mterm[0])) {
		mtype=models[0].var.type;
		if(locus<0) eff=tloc->eff[0];
		else if(mark->mterm[0]) {
			eff=mark->mterm[0]->eff;
			res_flag=1;
		}
	}
	for(j=comp=0;comp<n_comp;comp++) {
		for(k=0;k<2;k++) {
			mem_block[k]=first_mem_block[k];
			mem_block[k]->ptr=0;
		}
		if(locus<0)	{
			rf=r_func[n_markers][comp];
			n_all=nn_all;
			lump=0;
		} else {
			a_trans=allele_trans[locus][comp];
			rf=r_func[locus][comp];
			n_all=mark->n_all1[comp];
			if(n_all<2)	{
				j+=comp_size[comp];
				continue;
			}
			for(i=0;i<nn_all;i++) {
				for(grp=0;grp<n_genetic_groups;grp++) freq[grp][i]=0.0;
				peel_alls[i]=n_all-1;
			}
			for(i=0;i<nn_all;i++) if((k=a_trans[i])>=0) peel_alls[k]=i;
			for(grp=0;grp<n_genetic_groups;grp++)
			  for(i=0;i<nn_all;i++) freq[grp][peel_alls[i]]+=loc->freq[grp][i];
			lump=0;
			for(i=0;i<nn_all;i++) if(peel_alls[i]==n_all-1) lump|=(LK_ONE<<i);
		}
		n_peel_ops=0;
		cs=comp_size[comp];
		id=id_array+j;
		i=cs;
		i1=j;
		f_flag=founder_flag[locus1]+j;
		while(i--) {
			id->flag=0;
			id->allele[X_MAT]=id->allele[X_PAT]= -1;
			k3=*(f_flag++);
			if(k3==2) {
				id++;
				i1++;
				continue;
			}
			/* Set up transmission probs. */
			if(!k3 && n_loci>1) {
				for(k=idx-1;k>=0;k--) {
					k2=seglist[k][X_MAT][i1];
					if(k2<0) continue;
					theta=recom1[k];
#ifdef DEBUG
					if(theta<1.0e-16) {
						fprintf(stderr,"Warning: [1] theta very low (%g) for locus %d\n",theta,locus);
					}
#endif
					Mtp[k2]=1.0-theta;
					Mtp[1-k2]=theta;
					break;
				}
				if(k<0) Mtp[0]=Mtp[1]=1.0;
				for(k=idx-1;k>=0;k--) {
					k2=seglist[k][X_PAT][i1];
					if(k2<0) continue;
					theta=recom2[k];
#ifdef DEBUG
					if(theta<1.0e-16) {
						fprintf(stderr,"Warning: [2] theta very low (%g) for locus %d\n",theta,locus);
					}
#endif
					Ptp[k2]=1.0-theta;
					Ptp[1-k2]=theta;
					break;
				}
				if(k<0) Ptp[0]=Ptp[1]=1.0;
				for(k=idx;k<n_loci-1;k++) {
					k2=seglist[k][X_MAT][i1];
					if(k2<0) continue;
					theta=recom1[k];
#ifdef DEBUG
					if(theta<1.0e-16) {
						fprintf(stderr,"Warning: [3] theta very low (%g) for locus %d\n",theta,locus);
					}
#endif
					Mtp[k2]*=1.0-theta;
					Mtp[1-k2]*=theta;
					break;
				}
				for(k=idx;k<n_loci-1;k++) {
					k2=seglist[k][X_PAT][i1];
					if(k2<0) continue;
					theta=recom2[k];
#ifdef DEBUG
					if(theta<1.0e-16) {
						fprintf(stderr,"Warning: [4] theta very low (%g) for locus %d\n",theta,locus);
					}
#endif
					Ptp[k2]*=1.0-theta;
					Ptp[1-k2]*=theta;
					break;
				}
				z=1.0/(Mtp[0]+Mtp[1]);
				z1=1.0/(Ptp[0]+Ptp[1]);
				for(k=0;k<2;k++) {
					Mtp[k]*=z;
					Ptp[k]*=z1;
					id->tpp[X_MAT][k]=Mtp[k];
					id->tpp[X_PAT][k]=Ptp[k];
				}
				id->tp[X_MM_PM]=Mtp[X_MAT]*Ptp[X_MAT];
				id->tp[X_MM_PP]=Mtp[X_MAT]*Ptp[X_PAT];
				id->tp[X_MP_PM]=Mtp[X_PAT]*Ptp[X_MAT];
				id->tp[X_MP_PP]=Mtp[X_PAT]*Ptp[X_PAT];
			} else {	
				for(k=0;k<2;k++) id->tpp[X_MAT][k]=id->tpp[X_PAT][k]=0.5;
				for(k=0;k<4;k++) id->tp[k]=0.25;
			}
			id->rfp= -1;
			if(locus>=0) {
				id->lumped=mark->lump[i1];
				id->temp=mark->temp[i1];
				id->nhaps=mark->nhaps[i1];
				for(k=0;k<2;k++) {
					if(id->nhaps[k]==1) {
						a=id->temp[k];
						k1=1;
						while(!(a&1)) {
							a>>=1;
							k1++;
						}
						id->allele[k]=k1;
					}
				}
				if((id->ngens=mark->ngens[i1])==1) id->flag|=(SAMPLED_MAT|SAMPLED_PAT);
			}
			id++;
			i1++;
		}
		if(locus>=0 && mark->mterm && mark->mterm[0]) pen=&penetrance;
		n_rf= -1;
		sample_flag&=~OP_SAMPLING;
#ifdef TRACE_PEEL
		if(CHK_PEEL(TRACE_LEVEL_2)) (void)printf("Peeling component %d\n",comp+1);
#endif
		/* Go through all operations in peeling sequence */
		pp=peelseq_head[locus1]+comp;
		like1=0.0;
		while(pp->type) {
			if(pp->type==PEEL_SIMPLE) {
				simple_em=pp->ptr.simple;
				if(sample_flag && simple_em->pivot) 
				  peel_list[n_peel_ops++]=pp;
				if(locus<0)	{
					if(simple_em->out_index>n_rf)	{
						n_rf=simple_em->out_index;
						rf[n_rf].p=0;
					}
					z=loki_trait_simple_peelop(simple_em,locus,sample_flag,freq,rf,trait_pen,work);
					if(z== -DBL_MAX) {
						if(sample_flag&1) ABT_FUNC("Internal error - shouldn't be here\n");
						/* Error return on likelihood (not sampling) run.  Clean up and return error */
						return z;
					}
					like1+=z;
				} else {
					switch(linktype) {
					 case LINK_AUTO:
						z=loki_simple_peelop(simple_em,locus,sample_flag,*pen,a_set,freq,rf,work);
						break;
					 case LINK_X:
						z=loki_simple_peelop_x(simple_em,locus,sample_flag,*pen,a_set,freq,rf,work);
						break;
					 default:
						ABT_FUNC("Link type not implemented\n");
					}
					if(z== -DBL_MAX) {
						if(sample_flag&1) {
							ABT_FUNC("Internal error - shouldn't be here\n");
						}
						/* Error return on likelihood (not sampling) run.  Clean up and return error */
						return z;
					}
					like1+=z;
				}
				pp= &simple_em->next;
			} else {
				complex_em=pp->ptr.complex;
				if(sample_flag && (complex_em->n_involved-complex_em->n_peel))
				  peel_list[n_peel_ops++]=pp;
				if(locus<0)	{
					if(complex_em->out_index>n_rf) {
						n_rf=complex_em->out_index;
						rf[n_rf].p=0;
					}
					z=loki_trait_complex_peelop(complex_em,locus,sample_flag,rf,trait_pen,freq);
					if(z== -DBL_MAX) {
						if(sample_flag&1) ABT_FUNC("Internal error - shouldn't be here\n");
						/* Error return on likelihood (not sampling) run.  Clean up and return error */
						return z;
					}
					like1+=z;
				} else {	
					z=loki_complex_peelop(complex_em,locus,sample_flag,*pen,n_all,rf,freq);
					if(z== -DBL_MAX) {
						if(sample_flag&1) ABT_FUNC("Internal error - shouldn't be here\n");
						/* Error return on likelihood (not sampling) run.  Clean up and return error */
						return z;
					}
					like1+=z;
				}
				pp= &complex_em->next;
			}
		}
		/* If we're sampling, do them again in reverse */
		if(sample_flag) {
			sample_flag|=OP_SAMPLING;
			for(i=n_peel_ops-1;i>=0;i--) {
				pp=peel_list[i];
				if(pp->type==PEEL_SIMPLE) {
					simple_em=pp->ptr.simple;
					if(simple_em->pivot== -2) {
						k1=simple_em->out_index;
					} else {
						if(locus<0) (void)loki_trait_simple_sample(simple_em,locus,sample_flag,freq,rf,trait_pen,work);
						else {
							if(linktype==LINK_AUTO) (void)loki_simple_sample(simple_em,locus,*pen,a_set,freq,rf,work);
							else (void)loki_simple_sample(simple_em,locus,*pen,a_set,freq,rf,work);
						}
					}
				} else {
					complex_em=pp->ptr.complex;	
					k=0;
					if(complex_em->n_peel==1) {
						k1=complex_em->involved[0];
						if(locus>=0) {
							if(k1>0)	{
								if(id_array[k1-1].nhaps[X_MAT]==1) {
									k=1;
									id_array[k1-1].flag|=SAMPLED_MAT;
								}
							} else {
								if(id_array[-k1-1].nhaps[X_PAT]==1)	{
									k=1;
									id_array[-k1-1].flag|=SAMPLED_PAT;
								}
							}
						}
					}
					if(!k) {
						if(locus<0) (void)loki_trait_complex_peelop(complex_em,locus,sample_flag,rf,trait_pen,freq);
						else (void)loki_complex_peelop(complex_em,locus,sample_flag,*pen,n_all,rf,freq);
					}
				}
			}
			/* Get allele counts for frequency update */
			if(sample_freq) {
				f_flag=founder_flag[locus1]+j;
				if(locus>=0) {
					a_trans=allele_trans[locus][comp];
					for(id=id_array+j,i=0;i<cs;i++,id++) {
						if(*(f_flag++)!=1) continue;
						i1=i+j;
						grp=id->group-1;
						freq1=loc->freq[grp];
						count1=count[grp];
						for(k1=0;k1<2;k1++) {
							allele=id->allele[k1]-1;
#ifdef DEBUG
							if(allele<0 || allele>=n_all) {
								ABT_FUNC("Illegal sampled allele\n");
							}
#endif
							a=req_set[k1][locus][i1];
							if(a&(LK_ONE<<allele)) {
								z=0.0;
								k=0;
								while(a) {
									if(a&1) {
										k2=a_trans[k];
										if(k2<0) {
											b=lump;
											k3=0;
											while(b) {
												if(b&1) z+=freq1[k3];
												b>>=1;
												k3++;
											}
										} else z+=freq1[k2];
									}
									a>>=1;
									k++;
								}
								z=1.0/z;
								k=0;
								a=req_set[k1][locus][i1];
								while(a) {
									if(a&1) {
										k2=a_trans[k];
										if(k2<0) {
											b=lump;
											k3=0;
											while(b) {
												if(b&1) count1[k3]+=z*freq1[k3];
												b>>=1;
												k3++;
											}
										} else count1[k2]+=z*freq1[k2];
									}
									a>>=1;
									k++;
								}
							} else {
								k2=a_trans[allele];
								if(k2<0) {
									z=0.0;
									k3=0;
									b=lump;
									while(b) {
										if(b&1) z+=freq1[k3];
										b>>=1;
										k3++;
									}
									z=1.0/z;
									k3=0;
									b=lump;
									while(b) {
										if(b&1) count1[k3]+=z*freq1[k3];
										b>>=1;
										k3++;
									}
								} else count1[k2]+=1.0;
							}
						}
					}
				} else {
					for(id=id_array+j,i=0;i<cs;i++,id++) {
 						if(*(f_flag++)!=1) continue;
						grp=id->group-1;
						for(k1=0;k1<2;k1++) {
							allele=id->allele[k1]-1;
							count[grp][allele]+=1.0;
						}
					}
				}
			}
			/* Correct residuals, get segregation pattern */
			if(locus<0) { /* For trait loci */
				id=id_array+j;
				for(i=0;i<cs;i++,id++) {
					if(id->pruned_flag[n_markers]) continue;
					i1=i+j;
					k=id->allele[X_MAT];
					k1=id->allele[X_PAT];
					if(k>k1) k=k*(k-1)/2+k1;
					else k=k1*(k1-1)/2+k;
					/* Correct residuals for new genotypes */
					if(id->res[0]) {
						nrec=id->n_rec;
						k1=(loc->flag&LOCUS_SAMPLED)?loc->gt[i1]:1;
						if(k1!=k) {
							z=(k1>1)?eff[k1-2]:0.0;
							if(k>1) z-=eff[k-2];
							for(rec=0;rec<nrec;rec++) {
								id->res[0][rec]+=z;
							}
						}
					} 
					loc->gt[i1]=k;
					idd=id->dam;
					if(idd && !id_array[idd-1].pruned_flag[n_markers])	{
						k=id->allele[X_MAT];
						for(k1=0;k1<2;k1++)
						  trn[k1]=(k==id_array[idd-1].allele[k1])?1:0;
#ifdef DEBUG
						if(!trn[0] && !trn[1]) ABT_FUNC("Internal error - bad configuration?\n");
#endif
						if(trn[0] && trn[1])	{
							if(si || unlinked)	{
								tpp=id->tpp[X_MAT];
								z=ranf()*(tpp[0]+tpp[1]);
								seg[X_MAT][i1]=(z<=tpp[0])?0:1;
							} else seg[X_MAT][i1]= -2;
						} else if(trn[0]) seg[X_MAT][i1]=0;
						else seg[X_MAT][i1]=1;
					} else seg[X_MAT][i1]= -1;
					ids=id->sire;
					if(ids && !id_array[ids-1].pruned_flag[n_markers])	{
						k=id->allele[X_PAT];
						for(k1=0;k1<2;k1++)
						  trn[k1]=(k==id_array[ids-1].allele[k1])?1:0;
#ifdef DEBUG
						if(!trn[0] && !trn[1]) ABT_FUNC("Internal error - bad configuration?\n");
#endif
						if(trn[0] && trn[1])	{
							if(si || unlinked)	{
								tpp=id->tpp[X_PAT];
								z=ranf()*(tpp[0]+tpp[1]);
								seg[X_PAT][i1]=(z<=tpp[0])?0:1;
							} else seg[X_PAT][i1]= -2;
						} else if(trn[0]) seg[X_PAT][i1]=0;
						else seg[X_PAT][i1]=1;
					} else seg[X_PAT][i1]= -1;
				}
			} else { /* For marker loci */
				id=id_array+j;
				for(i=0;i<cs;i++,id++) {
					i1=i+j;
					if(id->pruned_flag[locus]) continue;
					k=id->allele[X_MAT];
					k1=id->allele[X_PAT];
					if(k>k1) k=k*(k-1)/2+k1;
					else k=k1*(k1-1)/2+k;
					if(res_flag) {
						if(id->res[0]) {
							nrec=id->n_rec;
							k1=(loc->flag&LOCUS_SAMPLED)?loc->gt[i1]:1;
							if(k1!=k) {
								z=(k1>1)?eff[k1-2]:0.0;
								if(k>1) z-=eff[k-2];
								for(rec=0;rec<nrec;rec++) {
									id->res[0][rec]+=z;
								}
							}
						}
					}
					loc->gt[i1]=k;
					idd=id->dam;
					if(idd && !id_array[idd-1].pruned_flag[locus]) {
						c=req_set[X_MAT][locus][i1];
						k=id->allele[X_MAT]-1;
#ifdef DEBUG
						if(k<0 || k>=n_all) ABT_FUNC("Internal error - bad sampled genotype\n");
#endif
						a=LK_ONE<<k;
						if(a&c) a=c;
						for(k1=0;k1<2;k1++) {
							k=id_array[idd-1].allele[k1]-1;
							b=LK_ONE<<k;
							trn[k1]=(a&b)?1:0;
						}
#ifdef DEBUG
						if(!trn[0] && !trn[1]) ABT_FUNC("Internal error - bad configuration?\n");
#endif
						if(trn[0] && trn[1])	{
							if(si)	{
								tpp=id->tpp[X_MAT];
								z=ranf()*(tpp[0]+tpp[1]);
								seg[X_MAT][i1]=(z<=tpp[0])?0:1;
							} else seg[X_MAT][i1]= -2;
						} else {
							seg[X_MAT][i1]=trn[0]?0:1;
						}
					} else seg[X_MAT][i1]= -1;
					ids=id->sire;
					if(ids && !id_array[ids-1].pruned_flag[locus]) {
						c=req_set[X_PAT][locus][i1];
						k=id->allele[X_PAT]-1;
#ifdef DEBUG
						if(k<0 || k>=n_all) ABT_FUNC("Internal error - bad sampled genotype\n");
#endif
						a=LK_ONE<<k;
						if(a&c) a=c;
						for(k1=0;k1<2;k1++) {
							k=id_array[ids-1].allele[k1]-1;
							b=LK_ONE<<k;
							trn[k1]=(a&b)?1:0;
						}
#ifdef DEBUG
						if(!trn[0] && !trn[1]) ABT_FUNC("Internal error - bad configuration?\n");
#endif
						if(trn[0] && trn[1]) {
							if(si)	{
								tpp=id->tpp[X_PAT];
								z=ranf()*(tpp[0]+tpp[1]);
								seg[X_PAT][i1]=(z<=tpp[0])?0:1;
							} else seg[X_PAT][i1]= -2;
						} else {
							seg[X_PAT][i1]=trn[0]?0:1;
						}
					} else seg[X_PAT][i1]= -1;
				}
			}
		}
		like+=like1;
		j+=cs;
	}
   if(sample_freq) for(grp=0;grp<n_genetic_groups;grp++) {
		z1=0.0;
		z=1.0;
		count1=count[grp];
		freq1=loc->freq[grp];	
		if(locus<0) {
			for(i=0;i<nn_all;i++) z1+=count1[i];
			for(i=0;i<nn_all-1;i++) {
				z1-=count1[i];
				freq1[i]=z*genbet(count1[i],z1);
				z-=freq1[i];
			}
			freq1[i]=z;
		} else {
			freq_set=mark->freq_set[grp];
			for(i=i1=0;i<nn_all;i++)	{
				if(freq_set[i]!=1) {
					z1+=count1[i];
					i1=i;
				} else z-=freq1[i];
			}
			for(i=0;i<i1;i++) if(freq_set[i]!=1) {
				z1-=count1[i];
				freq1[i]=z*genbet(count1[i],z1);
				z-=freq1[i];
			}
			freq1[i]=z;
		}
	}
	if(sample_flag) loc->flag|=LOCUS_SAMPLED|RFMASK_OK;
	return like;
}
