#include "pseud.h"

static void
CountConserveIndel(short ni, short nd, short lmax, short nmax, int target_d, short id0, int *num_cons);
static void
StoreConserveIndel(short ni, short nd, short lmax, short nmax, int target_d, short id0, PoolConsPtr pool_cons);
static short
SelectConserveIndelArray(PoolConsPtr pool_cons, CandStructPtr new_cand, short nmax, short lmax);
static void
SelectConservePosVector(CandStructPtr new_cand, int seqlen, int num_t);

static short
SelectConserveOrder(short *order, short ni, short nd);
static short
SelectConserveTvector(short *t_vector, int num_t, short nsum);

static void
CountConserveIndel(short ni, short nd, short lmax, short nmax, int target_d, short id0, int *num_cons)
{
	int   i, d, nind_index;
   int   max_iter, iter;
	short *temp_ins, *temp_del;
   		/*temp_ins is the array of the length of insertions
           temp_del is the array of the length of deletions*/

   temp_ins = (short *)calloc(nmax,sizeof(short));
   temp_del = (short *)calloc(nmax,sizeof(short));
   if(temp_del==NULL){printf("out of memory(press any key)\n");getch();}

   max_iter = 1;
   for(i=0;i<ni;i++){max_iter *= lmax;}
   for(i=0;i<nd;i++){max_iter *= lmax;}
   /*printf("max_iter=%d\n",max_iter);  */

   nind_index = (nmax+1)*ni + nd;

   /*initialization*/
   for(i=0;i<ni;i++){temp_ins[i]=1;}
   for(i=0;i<nd;i++){temp_del[i]=1;}
   iter=0;
   num_cons[nind_index]=0;
   while(iter<max_iter){
      d = CheckLengthChange(temp_ins, ni, temp_del, nd);
      if(d==target_d){
         if((id0>0)&&(temp_ins[0]==id0)){num_cons[nind_index]++;}
         if((id0<0)&&(temp_del[0]==-id0)){num_cons[nind_index]++;}
      }
      /*loop of del*/
      if(nd>0){
      	temp_del[nd-1]++;
      	CheckCarryUp(temp_del, nd, lmax);
      }
      if((temp_del[0]>lmax)||(nd==0)){
      	if(ni>0){
      		temp_ins[ni-1]++;
      		CheckCarryUp(temp_ins, ni, lmax);
   			for(i=0;i<nd;i++){temp_del[i]=1;}
         }
         if((temp_ins[0]>lmax)||(ni==0)){break;}
      }
      iter++;
   }
   /*printf("total_checked=%d\n",iter);  */
   /*printf("num_cons[%d]=%d\n",nind_index,num_cons[nind_index]);*/
   free(temp_ins); free(temp_del);

   return;

}

static void
StoreConserveIndel(short ni, short nd, short lmax, short nmax, int target_d, short id0, PoolConsPtr pool_cons)
{
	int   i, d, nind_index, product;
   int   num_passed, max_iter, iter;
	short *temp_ins, *temp_del;
   		/*temp_ins is the array of the length of insertions
           temp_del is the array of the length of deletions*/
   int  *num_cons;
	int  *i_pool_cons, *d_pool_cons;
	int   i_order, d_order;

   temp_ins = (short *)calloc(nmax,sizeof(short));
   temp_del = (short *)calloc(nmax,sizeof(short));
   if(temp_del==NULL){printf("out of memory(press any key)\n");getch();}

   max_iter = 1;
   for(i=0;i<ni;i++){max_iter *= lmax;}
   for(i=0;i<nd;i++){max_iter *= lmax;}
   /*printf("max_iter=%d\n",max_iter);  */

   nind_index = (nmax+1)*ni + nd;
   i_pool_cons = pool_cons->i_pool_cons;
   d_pool_cons = pool_cons->d_pool_cons;
   num_cons = pool_cons->num_cons;
   /*printf("num_cons=%d\n",num_cons[nind_index]);getch(); */

   /*initialization*/
   for(i=0;i<ni;i++){temp_ins[i]=1;}
   for(i=0;i<nd;i++){temp_del[i]=1;}
   iter=0;
   num_passed=0;
   while(iter<max_iter){
      d = CheckLengthChange(temp_ins, ni, temp_del, nd);
      if(d==target_d){
         if(((id0>0)&&(temp_ins[0]==id0))||((id0<0)&&(temp_del[0]==-id0))){
      		i_order = 0;
         	product = 1;
         	for(i=0;i<ni;i++){
         		i_order += temp_ins[i]*product;
            	product *= (lmax+1);
        	 	}
         	i_pool_cons[num_passed]=i_order;
         	d_order = 0;
         	product = 1;
         	for(i=0;i<nd;i++){
         		d_order += temp_del[i]*product;
            	product *= (lmax+1);
         	}
         	d_pool_cons[num_passed]=d_order;
      		num_passed++;
         }
      }
      /*loop of del*/
      if(nd>0){
      	temp_del[nd-1]++;
      	CheckCarryUp(temp_del, nd, lmax);
      }
      if((temp_del[0]>lmax)||(nd==0)){
      	if(ni>0){
      		temp_ins[ni-1]++;
      		CheckCarryUp(temp_ins, ni, lmax);
   			for(i=0;i<nd;i++){temp_del[i]=1;}
         }
         if((temp_ins[0]>lmax)||(ni==0)){break;}
      }
      iter++;
   }
   if(num_passed!=num_cons[nind_index]){printf("Warning!num_passed(%d)!=num_cons[nind_index](%d)\n",num_passed, num_cons[nind_index]);getch();}
   /*printf("total_checked=%d\n",iter);  */
   /*printf("num_cons[%d]=%d\n",nind_index,num_cons[nind_index]);*/
   free(temp_ins); free(temp_del);

   return;

}


PoolConsPtr
PoolConsDestruct(PoolConsPtr pool_cons)
{

	if(pool_cons==NULL){return NULL;}

   if(pool_cons->num_cons){pool_cons->num_cons = MemFree(pool_cons->num_cons);}
   if(pool_cons->i_pool_cons){
      pool_cons->i_pool_cons = MemFree(pool_cons->i_pool_cons);
   }
   if(pool_cons->d_pool_cons){
      pool_cons->d_pool_cons = MemFree(pool_cons->d_pool_cons);
   }
   pool_cons = MemFree(pool_cons);

   return pool_cons;

}


PoolConsPtr
PoolConsAllocate(int max_setnum, int max_stored)
{
	PoolConsPtr	pool_cons;
   int *i_pool_cons, *d_pool_cons;

   pool_cons = (PoolConsPtr)calloc(1,sizeof(PoolCons));
   if(pool_cons==NULL){printf("pool_cons allocation failed\n"); return NULL;}
   pool_cons->max_stored = max_stored;
   pool_cons->num_cons = (int *)calloc(max_setnum, sizeof(int));
   if(pool_cons->num_cons==NULL){
   	printf("No memory for num_cons\n");getch();pool_cons = MemFree(pool_cons); return NULL;
   }
   i_pool_cons = (int *)calloc(max_stored,sizeof(int));
   if(i_pool_cons==NULL){printf("No memory for i_pool_cons\n");pool_cons = PoolConsDestruct(pool_cons); return NULL;}
   pool_cons->i_pool_cons = i_pool_cons;
   d_pool_cons = (int *)calloc(max_stored,sizeof(int));
   if(d_pool_cons==NULL){printf("No memory for d_pool_cons\n");pool_cons = PoolConsDestruct(pool_cons); return NULL;}
   pool_cons->d_pool_cons = d_pool_cons;

   return pool_cons;
}

short
SelectConsNINDandPoolConsSetup(PoolConsPtr pool_cons, PoolStructPtr pool, CandStructPtr new_cand)
{
   int    *num_cons, num_passed;
   double *num_cand;
   short i, j, max_setnum;
   short ni, nd, i0, d0;
   short *order, *ins_array, *del_array;
   int   *limit_rij, select, nind_index;
   int   **i_pool, **d_pool;

   nind_index = (new_cand->ni)*(pool->nmax+1)+new_cand->nd;
   if(nind_index==0){printf("nind_index=0, You cannot conserve first element\n");getch();return 1;}

   max_setnum = pool->max_setnum;
   num_cons = pool_cons->num_cons;
   
   /*count the number of conserved candidates*/
	num_cand = pool->num_cand;
   order = new_cand->order;
   ins_array = new_cand->ins_array;
   del_array = new_cand->del_array;
   for(i=0;i<max_setnum;i++){
   	if(num_cand[i]<0){num_cons[i]=0;}
   	else{
      	ni = (short)(i / (pool->nmax+1));
   		nd = (short)(i % (pool->nmax+1));
   		if(order[0]==1){/*insertion*/
   			if(ni==0){num_cons[i]=0;}
      		else{
               num_passed = round(pow(10.0,num_cand[i]));
      			if(num_passed<(pool->max_stored)){
         			i_pool = pool->i_pool;
                  num_cons[i]=0;
            		for(j=0;j<num_passed;j++){
   						i0=(short)(i_pool[i][j]%(pool->lmax+1));
               		if(i0==ins_array[0]){num_cons[i]++;}
                  }
            	}
            	else{/*num_cand[i]>=(pool->max_stored)*/
               	CountConserveIndel(ni, nd, pool->lmax, pool->nmax, pool->target_d, ins_array[0], num_cons);
               }
         	}
         }/*end of if(order[0]==1)*/
   		if(order[0]==0){/*deletion*/
   			if(nd==0){num_cons[i]=0;}
      		else{
               num_passed = round(pow(10.0,num_cand[i]));
      			if(num_passed<(pool->max_stored)){
         			d_pool = pool->d_pool;
                  num_cons[i]=0;
            		for(j=0;j<num_passed;j++){
   						d0=(short)(d_pool[i][j]%(pool->lmax+1));
               		if(d0==del_array[0]){num_cons[i]++;}
                  }
            	}
            	else{/*num_cand[i]>=(pool->max_stored)*/
               	CountConserveIndel(ni, nd, pool->lmax, pool->nmax, pool->target_d, (short)(-del_array[0]), num_cons);
               }
         	}
         }/*end of if(order[0]==0)*/
      }
   }

   limit_rij = (int *)calloc(max_setnum+1,sizeof(int));
	limit_rij[0] = 0;
   for(i=0;i<max_setnum;i++){
      /*(ni,nd) should not go to itself*/
		if((i!=nind_index)&&(num_cons[i]>0)){limit_rij[i+1] = limit_rij[i]+1;}
      else{limit_rij[i+1] = limit_rij[i];}
      /*printf("limit_rij[%d]=%d\n",(i+1), limit_rij[i+1]);getch(); */
   }

   select = floor(rndu()*limit_rij[max_setnum]);
   /*printf("select = %d\n",select); */
   j = -1;
   for(i=0;i<max_setnum;i++){
   	if((select>=limit_rij[i])&&(select<limit_rij[i+1])){j=i;break;}
   }
   if(j==-1){printf("select nind_index failed\n");/*getch();*/return 1;}
   /*new nind_index=j*/
   ni = (short)(j / (pool->nmax+1));
   nd = (short)(j % (pool->nmax+1));
   /*printf("ni=%d, nd=%d\n",ni,nd); getch(); */
   new_cand->ni = ni;
   new_cand->nd = nd;
   nind_index = j;/*nind_index is updated*/

   if(num_cons[nind_index]>pool_cons->max_stored){
   	printf("num_cons[%d]=%d\n",nind_index,num_cons[nind_index]);getch();
   }

   if(order[0]==1){
		StoreConserveIndel(ni, nd, pool->lmax, pool->nmax, pool->target_d, ins_array[0], pool_cons);
   }
   if(order[0]==0){
		StoreConserveIndel(ni, nd, pool->lmax, pool->nmax, pool->target_d, (short)(-del_array[0]), pool_cons);
   }

   return 0;
}

static short
SelectConserveIndelArray(PoolConsPtr pool_cons, CandStructPtr new_cand, short nmax, short lmax)
{

	int   i, nind_index, select, i_order, d_order;
   int   *num_cons;
   int   *i_pool_cons, *d_pool_cons;
   short *ins_array, *del_array;

   nind_index = (nmax+1)*(new_cand->ni)+(new_cand->nd);
   num_cons = pool_cons->num_cons;
   /*printf("num_cons[%d]=%d\n",nind_index, num_cand[nind_index]);  */
   if(num_cons[nind_index]==0){/*printf("cannot change indel array\n");*/return 1;}

   i_pool_cons = pool_cons->i_pool_cons;
   d_pool_cons = pool_cons->d_pool_cons;
   ins_array = new_cand->ins_array;
   del_array = new_cand->del_array;
   if(num_cons[nind_index]==1){select=0;}
   else{select = floor(rndu()*num_cons[nind_index]);}
   i_order = i_pool_cons[select];
   d_order = d_pool_cons[select];

   for(i=0;i<(new_cand->ni);i++){
   	ins_array[i]=(short)(i_order%(lmax+1));
      i_order /= (lmax+1);
   }
   for(i=0;i<new_cand->nd;i++){
   	del_array[i]=(short)(d_order%(lmax+1));
      d_order /= (lmax+1);
   }


   return 0;

}

static short
SelectConserveOrder(short *order, short ni, short nd)
{
	int   i, p, nsum, max_order, num_order_cand, new_order, r, temp;
   short target_ni, num_i;
   short *temp_order;

	/*conserve order[0]*/
   if((order[0]==1)&&(nd==0)){for(i=0;i<ni;i++)order[i]=1; return 0;}
   if((order[0]==0)&&(ni==0)){for(i=0;i<nd;i++)order[i]=0; return 0;}

	nsum = ni + nd - 1;
   if(order[0]==1){target_ni = (short)(ni-1); }
   else{target_ni = ni;}

   /*num_order_cand = nsum C target_ni, max_order = 2^(nsum)*/
   num_order_cand = 1;
   for(i=0;i<target_ni;i++){num_order_cand *= (nsum - i);}
   for(i=target_ni;i>=1;i--){num_order_cand /= i;}
   /*printf("num_order_cand=%d\n",num_order_cand); */
   r = floor(rndu()*num_order_cand);
   /*search 'r'th order_index that satisfies num_i*/
   max_order = 1;
   for(i=0;i<nsum;i++){max_order *= 2;}

   temp_order = (short *)calloc(nsum,sizeof(short));
   p=-1;
   for(new_order=0;new_order<max_order;new_order++){
   	num_i = 0;
      temp = new_order;
   	for(i=0;i<nsum;i++){
      	temp_order[i] = (short)(temp%2);
         if(temp_order[i]){num_i++;}
         temp /= 2;
      }
      if(num_i==target_ni){p++;}
      if(p==r){
      	for(i=0;i<nsum;i++){order[i+1]=temp_order[i];}
         free(temp_order);
         return 0;
      }
   }
   free(temp_order);
   printf("conserve order selection failed\n");getch();
   return 1;


}

static short
SelectConserveTvector(short *t_vector, int num_t, short nsum)
{
	int i, p, max_t_index, num_t_cand;
   int r, temp, new_order, t1, t_num;
   short *temp_t;

   if(nsum<=0){return 1;}
	if(nsum==1){return 0;}

   t1 = -1;
   for(i=0;i<num_t;i++){if(t_vector[i]>0){t1=i;break;}}
   if(t1==-1){printf("cannot conserve t\n");getch();return 1;}

   if(nsum>(num_t-t1)){return 1;}
   /*num_t_cand = (num_t-t1-1)C(nsum-1)
     max_t_index = 2^(num_t-t1-1)
   */
   num_t_cand = 1;
   for(i=0;i<(nsum-1);i++){num_t_cand *= (num_t-t1-1-i);}
   for(i=(nsum-1);i>=1;i--){num_t_cand /= i;}

   max_t_index = 1;
   for(i=0;i<(num_t-t1-1);i++){max_t_index *= 2;}

   r = floor(rndu()*num_t_cand);
   temp_t = (short *)calloc(num_t-t1-1,sizeof(short));
   p=-1;
   for(new_order=0;new_order<max_t_index;new_order++){
   	t_num = 0;
      temp = new_order;
   	for(i=0;i<(num_t-t1-1);i++){
      	temp_t[i] = (short)(temp%2);
         if(temp_t[i]){t_num++;}
         temp /= 2;
      }
      if(t_num==(nsum-1)){p++;}
      if(p==r){
      	for(i=(t1+1);i<num_t;i++){t_vector[i]=temp_t[i-(t1+1)];}
         free(temp_t);
         return 0;
      }
   }
   free(temp_t);
   printf("select t_vector failed\n");getch();
   return 1;

}

static void
SelectConservePosVector(CandStructPtr new_cand, int seqlen, int num_t)
{
	int    p, t, current_seqlen;
   short  *id_array, *pos_vector;

   id_array = new_cand->id_array;
   pos_vector = new_cand->pos_vector;

   current_seqlen = seqlen;
   p=0;
   for(t=0;t<num_t;t++){
   	if(id_array[t]>0){
         if(p>0){pos_vector[p]=(short)floor(rndu()*(current_seqlen+1)); }
         p++;
         current_seqlen += id_array[t];
      }
      if(id_array[t]<0){
      	if(p>0){pos_vector[p]=(short)floor(rndu()*(current_seqlen+id_array[t]+1));}
         p++;
         current_seqlen += id_array[t];
      }
   }

	return;

}

short
SelectConserveChainTPos(PoolConsPtr	pool_cons, PoolStructPtr pool, CandStructPtr new_cand, CandStructPtr current_cand)
{
	short failed, loop;
   /*int   i;
   short *id_array, *pos_vector;*/

   /*printf("cuurent_cand\n");
   printf("(ni, nd)=(%d, %d)\nid_array:",new_cand->ni, new_cand->nd);
   id_array = new_cand->id_array;
   pos_vector = new_cand->pos_vector;
   for(i=0;i<(pool->num_t);i++){
   	printf("%d, ",id_array[i]);
   }
   printf("\npos_vector:");
   for(i=0;i<(new_cand->ni+new_cand->nd);i++){
   	printf("%d, ",pos_vector[i]);
   }
   printf("\n"); */

	failed=1;
   loop=0;
	while(failed){
		failed = SelectConsNINDandPoolConsSetup(pool_cons, pool, new_cand);
		/*printf("(ni,nd)=(%d,%d)\n",new_cand->ni,new_cand->nd); */
		if(!failed)failed =SelectConserveIndelArray(pool_cons, new_cand, pool->nmax, pool->lmax);
   	if(!failed)failed=SelectConserveOrder(new_cand->order, new_cand->ni, new_cand->nd);
   	if(!failed)failed=SelectConserveTvector(new_cand->t_vector, pool->num_t, (short)(new_cand->ni+new_cand->nd));
   	if(!failed){
      	SetupIDarray(new_cand, pool->num_t);
   		SelectConservePosVector(new_cand, pool->orig_seqlen, pool->num_t);
      }
   	loop++;
      if(failed)CopyCandidate(new_cand, current_cand, pool->num_t);
      if(loop>=11){printf("loop=%d\n",loop); return 1;}
	}
   /*printf("new_cand\n");
   printf("(ni, nd)=(%d, %d)\nid_array:",new_cand->ni, new_cand->nd);
   id_array = new_cand->id_array;
   pos_vector = new_cand->pos_vector;
   for(i=0;i<(pool->num_t);i++){
   	printf("%d, ",id_array[i]);
   }
   printf("\npos_vector:");
   for(i=0;i<(new_cand->ni+new_cand->nd);i++){
   	printf("%d, ",pos_vector[i]);
   }
   printf("\n"); */
   return 0;
}