/* 
 * Thomas Pantzer: t_deq_l.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 <stdio.h>
#include <math.h>

#include "copyright.h"

#include "t_deq_l.h"
#include "trigonom.h"
#include "graphix.h"


void t_deq_l::Init(int i, double a, t_deq *N)
{
	t_deq::Init(i,a,N);
	type = e_line;	

	double step =  (double)i * a * 2.0 * M_PI;
	param[1] = cos(step);
	param[2] = sin(step);

	dg_angle = atan2(param[2],param[1]);
	old_angle = dg_angle;
	rise_left = -1.0;
	rise_right = -1.0;
}


double t_deq_l::Error(t_vector &Df,  double ds, double da_avg, double ds_avg)
{
	if ((rise_left > 0.0) && (rise_right > 0.0))
	   return ErrorCos3(Df,ds,da_avg,ds_avg);

	// we choose the used errorfunction from setting this variables
	// to a resonable value
	

	double a = SQR(param[2] - (Df.y/ds));
	double b = SQR(param[1] - (Df.x/ds)); 
	err = (a + b)/4.0;

	W += ((err - (2.0*W)) * (1.0/Tw));

#ifdef DEBUG
	if ((a < 0.0) || (b < 0.0))
	    fprintf(stderr,"quadrat < 0.0 (%g,%g)\n",a,b);	
#endif 
	return err;
}



inline double ___sign(double x)		// the sign of a number
{
	if (x > 0.0)
	   return 1.0;

	return 0.0;
//	return fabs(x)/(2.0*x)+0.5;
}

inline double ___delta2rise(double x)
{
	return (M_PI+3.0) / ((fabs(x) + 0.001) * 2.0);
}

inline double ___cos3(double x)
{
	double d = -cos(x);
	double r = d * d * d;	// a nice cosine
				// with smooth mid-level
				// more rise than normal cosine 	
//	return (-3*cos(x)-cos(3*x))/4;
	return r;
}

inline double ___window(double x)	// a window function at pos x
{				// 1.0 for a length of 2*Pi (starting from -Pi)
				// else 0.0
	return ___sign(x)*(1.0-___sign(x-(2.0*M_PI)));
}

inline double ___wl(double x)		// the left side of that window
{
	return ___window(2.0*(x+M_PI));
}

inline double ___wr(double x)		// the right side of that window
{
	return ___window(2*x);
}

inline double ___wls(double x,double y,double m)
{				// left side of window scaled with rise ``m''	
	return ___wl(m*(x-y));	// at pos ``x'' relative shifted by ``y''
}

inline double ___wrs(double x,double y,double m)
{				// right side version	
	return ___wr(m*(x-y));
}

inline double ___wgs(double x,double y,double l,double r)
{				// the outside of the window
				// 0.0 at window, 1.0 else
				// at pos x shifted by y with 
				// left and right rise scalings
	return 1.0 - ___wls(x,y,l) - ___wrs(x,y,r);
}

inline double ___ts(double x,double y,double m)
{			// shifted ``y'', rise ``m'' scaled, nice cosine
			// at centerposition ``x''
	return ___cos3(m*(x-y));
}

inline double ___tgs(double x,double y,double l,double r)
{			// nice cosine with left and right rise scale
			// one period at x shifted b y
	return (___ts(x,y,l)*___wls(x,y,l)) +  // the left side cosine * left window
		(___ts(x,y,r)*___wrs(x,y,r) +  // the right side cosine * right window
		  ___wgs(x,y,l,r));	       // the outer window ( == 1.0)		
}

inline double ___ct(double x,double y,double l,double r)
{			// three periods of the above funtion
	return ___tgs(x,y-2.0*M_PI,l,r) *
		___tgs(x,y,l,r) * 
		 ___tgs(x,y+2.0*M_PI,l,r);
}

inline double ___cts(double x,double y,double l,double r)
{			// scaled to the range from 0.0 to 1.0
	return (___ct(x,y,l,r)+1.0)/2.0;
}


double t_deq_l::ErrorCos3(t_vector &Df,  double ds, double da_avg, double ds_avg)
{
//	double a = atan2(param[2],param[1]);
// 	if ((left == NULL) || (right == NULL))
// 	   return Error(Df,ds,da_avg,ds_avg);

	double b = atan2((Df.y/ds),(Df.x/ds));

	err = ___cts(dg_angle,b,rise_left,rise_right);

	W += ((err - (2.0*W)) * (1.0/Tw));

	return err;
}

double t_deq_l::Angle_Drift(char reset)
{
	double S = 0.0;
       
	dg_angle = atan2(param[2],param[1]);
        
	double diff = dg_angle - old_angle;

	if (reset == true)
           old_angle = dg_angle;
          
	S += RAD_NORM(diff);
        
	return S;
}


char t_deq_l::ComparesTo(t_deq *other)
{
   	other->dg_angle = atan2(other->param[2],other->param[1]);
  
        double a = angle_diff(other->dg_angle);

	if (a > (M_PI/180.0))
	   return false;

        double e = fabs(err - other->err);
	if (e > 0.01)
	   return false;

        double f = fabs(fit - other->fit);
	if (f > 0.01)
	   return false;
	
	fprintf(stderr,"two t_diffgl match a:%g° e:%g f:%g\n",RAD(a),e,f);

	return true;
}


void t_deq_l::Show(t_vertex_2i *vcenter, double size_, t_deq *BP, t_deq *BF, t_deq *SE)
{
	char st[256];
	t_vector EP;
	t_vertex_2i center;
	double s;

	center.x = vcenter->y+20;
   	center.y = vcenter->y;

	s = ((VScr.size/2.0) - 20.0);
	dg_angle = atan2(param[2],param[1]);

	EP.x = cos(dg_angle) * s;
	EP.y = sin(dg_angle) * s;
	EP.y = -EP.y;

	EP.x += center.x;
	EP.y += center.y;

	if (BP == this)
           SetColor(FL_YELLOW);
	 else
           SetColor(FL_DARKORANGE);

	drawline(center.x,center.y,(int)EP.x,(int)EP.y);

	sprintf(st,"%d. p:%1.3f; e:%1.3f; f:%1.3f",id,prob,err,fit);
	SetColor(FL_CYAN);
	OutTextXY((int)EP.x,(int)EP.y,st);

	if (SE == this)
           {
            SetColor(FL_BLUE);
            DrawCircle((int)(EP.x),(int)(EP.y),8);
           } 

}


void t_deq_l::Update(double factor, t_vector &Df, double ds, double da_avg, double ds_avg)
{
	param[1] += (factor * ((Df.x/ds) - param[1]));
	param[2] += (factor * ((Df.y/ds) - param[2]));

   	dg_angle = atan2(param[2],param[1]);
}


double t_deq_l::Diff2Neighbour(t_deq_l *N, char LR)
{
        double a = angle_diff(N->dg_angle);

	if (LR == true)
	   rise_left = ___delta2rise(a);
	 else
	   rise_right = ___delta2rise(a);

	return a;
}















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