#include "pseud.h"
#include <math.h>

static void
Estimatelkl(ScoreBlkPtr score_block, OptionsBlkPtr options, int pos);

ScoreBlkPtr
ScoreBlkFree(ScoreBlkPtr score_block)
{
	if(score_block==NULL){return NULL;}

   if(score_block->lkl){
   	score_block->lkl = MemFree(score_block->lkl);
   }
   if(score_block->anc_no){
   	score_block->anc_no = MemFree(score_block->anc_no);
   }
   if(score_block->P){
   	score_block->P = MemFree(score_block->P);
   }
   score_block = MemFree(score_block);

	return score_block;
}
ScoreBlkPtr
SetupScoreblock(OptionsBlkPtr options, CandStructPtr new_cand, int count_per_cycle)
{
	ScoreBlkPtr score_block;
   int     i, p, t, seqlen;
   short   *id_array;
   short  *pos_vector;
   int    *anc_no;

   score_block = (ScoreBlkPtr)calloc(1,sizeof(ScoreBlk));
   if(score_block==NULL){return NULL;}

   score_block->target_seqlen = options->target_len;
   score_block->lkl = (double *)calloc(score_block->target_seqlen,sizeof(double));
   if(score_block->lkl==NULL){score_block=MemFree(score_block);return NULL;}
   score_block->anc_no = (int *)calloc(score_block->target_seqlen+(options->nmax)*(options->lmax),sizeof(int));
   if(score_block->anc_no==NULL){score_block=ScoreBlkFree(score_block);return NULL;}
   score_block->count_per_cycle = count_per_cycle;
   score_block->max_cycle=1;
   score_block->current_cycle=0;
   score_block->P = (double *)calloc(score_block->max_cycle,sizeof(double));
   if(score_block->P==NULL){score_block=ScoreBlkFree(score_block);return NULL;}
   score_block->MP=0;
   score_block->VP=0;

   /*anc_no*/
   id_array = new_cand->id_array;
   pos_vector = new_cand->pos_vector;
   anc_no = score_block->anc_no;
   seqlen = options->orig_len;
   for(i=0;i<seqlen;i++){anc_no[i]=i;}
	p=0;
   for(t=0;t<options->num_t;t++){
   	if(id_array[t]>0){/*insertion*/
         seqlen += id_array[t];
         for(i=(seqlen-1);i>=pos_vector[p];i--){
         	anc_no[i+id_array[t]]=anc_no[i];
         }
         for(i=0;i<id_array[t];i++){anc_no[pos_vector[p]+i]=-1;}
         p++;
      }
      if(id_array[t]<0){ /*deletion*/
      	seqlen += id_array[t];
         for(i=pos_vector[p];i<seqlen;i++){
         	anc_no[i]=anc_no[i-id_array[t]];
         }
         p++;
      }
   }
   /*if(seqlen!=(score_block->target_seqlen)){
   	printf("Warning!anc_no seqlen = %d, target_seqlen=%d\n",seqlen,(score_block->target_seqlen));
   	getch();
   }*/



   return score_block;

}

void
Initializelkl(ScoreBlkPtr score_block)
{
   int      i;
   double   *lkl;

   /*initialize lkl */
   lkl = score_block->lkl;
   for(i=0;i<score_block->target_seqlen;i++){lkl[i]=0;}

   return;
}

void
Scorelkl(Uint1Ptr target_seq, Uint1Ptr simul_seq, ScoreBlkPtr score_block)
{
	int i, seqlen;
   double *lkl;

   seqlen = score_block->target_seqlen;
   lkl = score_block->lkl;
   for(i=0;i<seqlen;i++){
   	if(target_seq[i]==simul_seq[i]){lkl[i]+=1.0;}
   }

   return;
}

static void
Estimatelkl(ScoreBlkPtr score_block, OptionsBlkPtr options, int pos)
{
   double   *lkl;
	int *anc_no;
   Uint1Ptr orig_seq;
   Uint1Ptr target_seq;
   int      b1, b2;
   double   *PMat;

   lkl = score_block->lkl;
   anc_no = score_block->anc_no;
   orig_seq = options->orig_seq;
   target_seq = options->target_seq;
   if(anc_no[pos]<0){/*probability of each base of insertion*/
      lkl[pos]=(1-(options->x+options->y)/(1+options->y))*0.25;
      /*This will be modified later*/
   }
   else{
   	b1 = (int)(orig_seq[anc_no[pos]]-1);
      b2 = (int)(target_seq[pos]-1);
      PMat = options->PMat;
      lkl[pos]=PMat[NASize*b1+b2];
   }

   return;

}

short
CalculateScoreBlock(ScoreBlkPtr score_block, OptionsBlkPtr options, FILE *fout)
{
	int i, seqlen;
   int current_cycle;
   double *lkl;
   double *P;
   double MP, VP;

   /*printf("Calculate lkl, current_cycle = %d\n",score_block->current_cycle); */
   lkl = score_block->lkl;
   seqlen = score_block->target_seqlen;
   P = score_block->P;
   current_cycle = score_block->current_cycle;

   /*fprintf(fout,"current_cycle=%d\n",current_cycle); */
   P[current_cycle]=0;
   for(i=0;i<seqlen;i++){
   	if(lkl[i]==0){
      	Estimatelkl(score_block, options, i);
      	/*fprintf(fout,"*"); */
      }
      else{lkl[i] /= (score_block->count_per_cycle);}
      if(lkl[i]<=0){P[current_cycle]+=MinlnL;}
      else{P[current_cycle]+=log10(lkl[i]);}
      /*fprintf(fout,"%2.4f,",log10(lkl[i])); */
   }
   /*fprintf(fout,"\nP[%d]=%2.4f\n",current_cycle, P[current_cycle]); */

   if(current_cycle==0) {MP = P[0]; VP=10;}
   if(current_cycle>0){
   	MP=0;
      for(i=0;i<=current_cycle;i++){MP += pow(10.0, P[i]);}
      MP /= (current_cycle+1);
      VP=0;
      for(i=0;i<=current_cycle;i++){
      	VP += (pow(10.0, P[i])-MP)*(pow(10.0, P[i])-MP);
      }
      VP /= (current_cycle+1);
      if(MP<=0){printf("MP(%lf)<=0\n",MP);MP = MinlnL;}
      else{MP = log10(MP); }
      if(VP<=0){printf("VP(%lf)<=0\n",VP);VP = MinlnL;}
      else{VP = log10(VP); }
   }

   score_block->MP = MP;
   score_block->VP = VP;
   current_cycle++;
   score_block->current_cycle = current_cycle;
   if(current_cycle==score_block->max_cycle){return 1;}

   return 0;

}