/* 
 * Thomas Pantzer: t_woeq.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1999 by Thomas Pantzer
 * 
 * Permission to use, copy, modify, and distribute this software for noncommercial 
 * use and without fee is hereby granted, provided that the above copyright notice 
 * appear in all copies and that both that copyright notice and this permission 
 * notice appear in supporting documentation, and that the name of the Author not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The Author makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@isys2.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */




#include <math.h>
#include <fcntl.h>
#include <unistd.h>

#include "copyright.h"

#include "t_woeq.h"
#include "trigonom.h"
#include "diffeq_demo.h"

#include "t_deq_l.h"
#include "t_sorter.h"

void t_woeq::Init(void)
{	
	count = 0;
	_epsilon = 0.031;
	_NY = 1.0;
	_Info_TH = 0.16;
	min_info = 3.0;

	first = NULL;
	delay.Init(100,1,16);
	no_update.Init(5,10,16);
	shift = 100;

//	max_dcnt = 0;
	phi = 0.0;
	gamma = 0.0; //0.0000575;  // 0.0; //(-0.545/18000);
	v_gamma = 0.0;
	kappa = 0.0;
	ignore_gamma = true;
	gamma_threshold = 0.33;
//	sg = 0.0;
//        gamma_pre_pos.y = 0.0;

//        prob_pre_pos.x = 0.0;
//        prob_pre_pos.y = 0.0;

	_DRate = 50000.0;
}

void t_woeq::Done(void)
{
	delay.Done();
	no_update.Done();
}


void t_woeq::Add(t_deq *p)
{
	if (p == NULL)
	   return;

//	p->next = first;
	first = p;
	count++;
	pMin = (1.0 - pMaxMax) / (count -1);

	if (count > 2)
           {
            double n = (double)(count-1);
	    double v = 1.0 / n;
  	    gamma_threshold = -(log(v)*v) / log(n);
#ifdef DEBUG
	    fprintf(stderr,"gamma_th: %g, v:%g, n:%g, count: %d\n",
                          gamma_threshold,v,n,count);	
#endif
           } 
}

void t_woeq::Del(t_deq *p)
{
	if (p == NULL)
	   return;

	t_deq *deqp = first;

	if (p == first)
	   { // head delete
	    first = first->next;
	    deqp->next = NULL;
	    deqp->Done();
	    delete deqp;
   	   }
         else
           {
	    while (deqp->next != p)
               deqp = deqp->next;

	    if (deqp->next != NULL)
               {
                t_deq *delp = deqp->next;
	        deqp->next = deqp->next->next;

		delp->next = NULL;
		delp->Done();
		delete delp;
               }	   
           }

	count--;
	pMin = (1.0 - pMaxMax) / (count -1);
	if (count > 2)
           {
            double n = (double)(count-1);
	    double v = 1.0 / n;
  	    gamma_threshold = -(log(v)*v) / log(n);
           } 
}

double t_woeq::ErrSum(t_vector &Df, double ds, double da_avg, double ds_avg)
{
   double S;
   t_deq *DG;
   
   S = 0.0;

   DG = first;
   while (DG != NULL)
     {
      S += exp(-DG->Error(Df,ds,da_avg,ds_avg));
      
      DG = DG->next;
     }
   
   return S;
}

double t_woeq::AvgFit(void)
{
   double S;
   int i;
   t_deq *DG;
   
   S = 0.0;
   i = 0;

   DG = first;
   while (DG != NULL)
     {
      S += (DG->Fit(err_sum) * DG->prob);
      i++;
      
      DG = DG->next;
     }
   
   return ( S / (double)i);
//   return S;
}


t_deq *t_woeq::GetBestFit(void)
{
   t_deq *DG,*Best;
//   double max;
//   int i;
   
   DG = first;
   Best = DG;
   
   while (DG != NULL)
     {
      if (DG->fit > Best->fit)
         Best = DG;
         
      DG->best = false;         
      DG = DG->next;   
     }   
          
   Best->best = true;       
   return Best;       
}

t_deq *t_woeq::GetBestProbability(void)
{
   t_deq *DG,*Best;
//   double max;
//   int i;
   
   DG = first;
   Best = DG;
   
   while (DG != NULL)
     {
      if (DG->prob > Best->prob)
         Best = DG;
         
      DG->best = false;         
      DG = DG->next;   
     }   
          
   Best->best = true;       
   return Best;       
}


t_deq *t_woeq::GetSmallestError(void)
{
   t_deq *DG,*Best;

   DG = first;
   Best = DG;
   
   while (DG != NULL)
     {
      if (DG->err < Best->err)
         Best = DG;
         
      DG->best = false;         
      DG = DG->next;   
     }   
          
   Best->best = true;       
   return Best;       
}

double t_woeq::Prob_Info(void)
{
   double S;
   t_deq *DG;
   
   S = 0.0;
   DG = first;
   
   while (DG != NULL)
     {
      S += (DG->prob * log(DG->prob));

      DG = DG->next;
     }

   if (count > 1)
      pMin = (1.0 - pMaxMax) / (double)(count - 1);

   double i = -S/log((double)count);

   if (i < min_info)
      min_info = i;

   return i;	
}

double t_woeq::Error_Info(void)
{
	double S = 0.0;
	t_deq *DG = first;
   
	while (DG != NULL)
	  {
	   S += (DG->err * log(DG->err));
	   DG = DG->next;
	  }

	return -S/log((double)count);	
}


void t_woeq::Norm_Probability(void)
{
	double S = 0.0;
	t_deq *DG = first;

	while (DG != NULL)
	  {
	   S += DG->prob;
	   DG = DG->next;
	  }

	DG = first;
	while (DG != NULL)
	  {
	   DG->prob /= S;
	   DG = DG->next;
	  }
}

void t_woeq::Show(t_vertex_2i *vcenter, double size_)
{
	// BF = GetBestFit()
        // SE = GetSmallestError()

	t_deq *p = first;
	while (p != NULL)
	  {
	   p->Show(vcenter,size_,BP,BF,SE);
           p = p->next;
	  }	

	SetColor(FL_GREEN);
	t_vector EP;
	EP.x = cos(phi) * (size_/2.0);
	EP.y = sin(phi) * (size_/2.0);
        EP.x += vcenter->x;
	EP.y += vcenter->y;
	drawline(vcenter->x,vcenter->y,(int)EP.x,(int)EP.y);

	SetColor(FL_WHITE);
        char st[256];

	sprintf(st,"gamma:%g",gamma);
	OutTextXY((int)EP.x,(int)EP.y,st);

	sprintf(st,"info:%g",info);
	OutTextXY(20,(int)size_ - 20,st); 

	sprintf(st,"shift:%g",gamma);
	OutTextXY(20,(int)size_ - 40,st);
	
}

double t_woeq::Angle_Drift(char reset)
{
	t_deq *p = first;

	double S = 0.0;

	while (p != NULL)
	  {
           if (p->type == e_line)
	      S += p->Angle_Drift(reset);

	   p = p->next;
	  }

	return S / (double)count;
}


double Criterion_2(void *Ptr)
{
	t_deq *dg = (t_deq *)Ptr;
        return dg->param[2]; 
}

double Criterion_4(void *Ptr)
{
	t_deq_l *dg = (t_deq_l *)Ptr;
        return dg->dg_angle;
}


void t_woeq::Use_oldErrorFunc(void)
{
	t_deq_l *DG = (t_deq_l *)first;
	while (DG != NULL)	
 	  {
	   if (DG->type == e_line)
	      {
	       DG->rise_left = -1.0;
	       DG->rise_right = -1.0;
	      }	

	   DG = (t_deq_l *)DG->next;
	  }			// count the number of lines
}

void t_woeq::NeighbourUpdate(void)
{
	int k = 0;
	t_deq *DG = first;
	while (DG != NULL)	
 	  {
	   if (DG->type == e_line)
		k++; 	

	   DG = DG->next;
	  }			// count the number of lines
   
	if (k <= 0)
	   return;

	t_deq_l **lar = new t_deq_l* [k]; 
	DG = first;
	k = 0;
	while (DG != NULL)
 	  {
	   if (DG->type == e_line)
	      {
	       lar[k] = (t_deq_l*)DG; 		// form an array of pointers
	       k++; 	
	      } 

	   DG = DG->next;
	  }

	t_sorter *lrsort = new t_sorter;

	lrsort->Init((void**)lar,Criterion_4,k,true);
	lrsort->Run();			// quicksort this array
	lrsort->Done();			// by degree

	delete lrsort;			

	for (int n=0; n<k; n++)
	  {
	   int pre = (n + k - 1) % k;
	   int post = (n + 1) % k;

	   lar[n]->Diff2Neighbour(lar[pre],true); 	 
	   lar[n]->Diff2Neighbour(lar[post],false); 	 
	  }

	delete [] lar; 	
}

void t_woeq::ProbUpdate(t_vector &Ds, double da_avg, double ds_avg, char f2)
{
	Ds.Rotate(-phi);
   
	err_sum = ErrSum(Ds,Ds.Length(),da_avg,ds_avg);   // ,&prob_pre_pos
	avg_fit = AvgFit();

	if (f2)
	   info = Error_Info();
	 else
	   info = Prob_Info();
   
	t_deq *DG = first;
	char norm_req = false;
	while (DG != NULL)
	  {
	   if (DG->Update(_NY,avg_fit,pMin) == false)
	      norm_req = true;

	   DG = DG->next;
	  }
   
	if (norm_req == true)
	   Norm_Probability();   
}



char *t_woeq::write_log(double da_avg, double ds_avg)
{
//   char st[4096];
   char *st = new char [4096]; 	

   char st2[4096];
   sprintf(st,"%g\t%g\t%g\t%g\t%g\t%g",info,gamma,v_gamma,_DRate,da_avg,ds_avg/*,s*/);

   {
   int i = 1;
   t_deq *DG = first;
   while (DG != NULL)
     {
      sprintf(st2,"\t%g\t%g\t%g",DG->err,
	  			 DG->prob,
		                 expm1(-DG->W/0.1) + 1.0);
//      DG->rho = ;
      strcat(st,st2);
      DG = DG->next;
      i++;
     }
   }

   strcat(st,"\n");
//   write(fd_gamma_log,st,strlen(st));
	return st;

//   gamma_pre_pos.x = pos.x;
//   gamma_pre_pos.y = pos.y; 
//   sprintf(st,"%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g",
//              S,p,gamma,kappa,phi,drift,info,min_info,BF->prob,BP->prob,BF->fit); 

}

void t_woeq::Statistics(void)
{
   BP = GetBestProbability();     
   SE = GetSmallestError();
   BF = GetBestFit();  	

   BF->quality += (BF->fit * BF->prob);
   drift = Angle_Drift(false);
  
   no_update.Start(info > _Info_TH);

   if (delay.Start((BF != BP) && (info < _Info_TH)))
      {
#ifdef DEBUG
       fprintf(stderr,"no_upd: %g\tdelay: %g\tdrift: %g°\tshift: %d\n",
                     no_update.avg.Value(),delay.avg.Value(), DEG(drift), shift);  
#endif
//       shift = (int)((no_update.avg * (double)no_update.F)/2.0);
//       shift += (int)delay.avg;	
//       shift -= (int)no_update.avg;	
       shift = (int)delay.avg.Value();
      }

   no_update.Count(BF != BP); 
   delay.Count(BF != BP);
}

char t_woeq::GammaUpdate(t_vector &Ds, double da_avg, double ds_avg)
{
	Statistics();
	Ds.Rotate(-phi);	

	double ds = Ds.Length();  // SQR(dx) + SQR(dy);

	char upd = false;
	t_deq *DG = first;
	double S = 0.0;
  
	if (ignore_gamma == true)
	   {
	    int i = 0;
	    double V[20],max;
	    max = 0.0;
	
	    while (DG != NULL)
	      {
	       V[i] = fabs(DG->quality)+0.000001;
	       max += V[i];

	       i++;
	       DG = DG->next; 
	      }

	    for (int j=0; j<i; j++)
	      S += (log((V[j]/max)) * (V[j]/max)); 

	    S = -S;
	    S /= (double)i;	
	    if (S > gamma_threshold)
	       {
	        if (gamma_correct != 0)
	           ignore_gamma = false;
//	   phi = Angle_Drift(true);
	       }

	   DG = first;	
	  }	


	if (info < _Info_TH)	
//	if (no_update.running == true)
//	if (info < (min_info*1.5))
	   {//  update  params
	    upd = true;
//       fprintf(stderr,"count:%d\n",count);
//       BF->Update(BF->GetFactor(epsilon,avg_fit,count),dx,dy,ds);
//       DG = BF;
//       double K = DRate;
//       double K = DRate;

	    while (DG != NULL)
              {
               double sgp = sigmoid(DG->prob,30.0,0.7);
	       double factor = _epsilon * sgp;
               DG->Update(factor,Ds,ds,da_avg,ds_avg);

	       double dsx = Ds.x / ds;
	       double dsy = Ds.y / ds;

               double in_braces = ((dsx - DG->param[1]) * dsy)  
        	        	   - ((dsy - DG->param[2]) * dsx) ;

               gamma += (-factor*ds/_DRate) * in_braces;

     //              kappa += (-factor*da_avg/K) * in_braces;
     //	  DG->rho = expm1(-DG->W/1.0) + 1.0;

               DG = DG->next;
              }
	   } 

	double p = (gamma * ds);          //   double s = (kappa * da_avg);
	v_gamma += (p - v_gamma)/300.0;	

	if (ignore_gamma == false)
	   phi += p;

	return upd;   
}



















/**/
/**/
/**/
/**/
/**/
/**/
/**/
/**/
/*****  End Of File  ****************************/
