/* 
 * University of Leipzig: trigonom.cc, v0.1b  21.9.97 22:58 
 * 
 * 
 * Copyright 1997 by University of Leipzig
 * 
 * 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 U.L. not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The U.L. makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@aix520.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */

#include <math.h>
#include <stdlib.h>
#include <stdio.h>


#include "copyright.h"

#include "khepera.h"
#include "trigonom.h"
//#include <p2c/p2c.h>



int random_int(int range)
{
  /* rand() returns number from  0 to 2^31-1 ( = 2147483647 ) */
  /* return((int)floor((double)random()/2147483648.0*(double)range)); */
  /* return((int)floor((double)rand()*(double)range));*/

  /* rand() returns number from  0 to 2^15-1 ( = 32767 ) */  
   return((int)floor((double)rand()/(RAND_MAX+1)*(double)range));     
}

/**************************************************************************/

double random_uniform()
{
  /*return(rand());
  return(random()/2147483648.0);   */
  double d;
  
  d = rand() / (RAND_MAX+1.0);    

/*
  if (d < 0.0)
      return -d;
    else
 */ 
#ifdef DEBUG_RAND
  fprintf(stderr,"%g == rand_uniform()\n",d);  
#endif 

  return d;
}


double angle_average(double deg, double deg2)
{
          double n2 = (deg2 - deg)/2.0;
          if (fabs(n2) > (M_PI/2.0))
              n2 = RAD_NORM(n2 + deg);
           else   
              n2 = RAD_NORM(n2 + deg - M_PI);

	  return n2;
}


double angle_difference(double v, double w)
{
   double diff;
   double p0 = RAD_NORM(v) - RAD_NORM(w);

   diff = fabs(p0);
          
   if (diff > M_PI)
      diff = (2*M_PI) - diff;
      
   return diff;   
	
}



double clip_angle(double v)
{
  double w = v;
  
  while (w < 0)
      w += (2*M_PI);

  while (w > (2*M_PI))
      w -= (2*M_PI);

  if (w>M_PI)
     w = w - (2.0*M_PI);
      
  return w;    
}

double RAD_NORM(double a)
{
   double al = a;
   
   while (al > M_PI)
     al -= (2.0 * M_PI);

   while (al < -M_PI)
     al += (2.0 * M_PI);
     
   return al;  
}

double DEG_NORM(double a)
{
   double al = a;
   
   while (al > 180.0)
     al -= 360.0;

   while (al < -180.0)
     al += 360.0;
     
   return al;  
}
	
/*
double distance(t_vertex_2d v1,t_vertex_2d v2)
{
    double d,a,b,c,e;
    
    a = (v1.x - v2.x);
    b = (v1.y - v2.y);

    c = SQR(a);
    d = SQR(b);

    e = sqrt(c+d);
#ifdef DEBUG
    fprintf(stderr,"distance = sqrt(%g) = %g\n",c+d,e);
#endif
    return e;
}
*/

double gaussian(double x,double b, double m)
{
  double g;
  
  g = m*(expm1(-SQR(x/b))+1.0);

  return g;
}

/*
static inline double _sigmoid(float x)
{
	return (1.0 -  (1.0/(1.0+(double)exp(x))));   
}
*/

#define _sigmoid(x) (1.0 -  (1.0/(2.0+expm1(x))))

double sigmoid(double x,double acc,double shift)
{
	return _sigmoid(acc*(x - shift));
}


void HalfWay(t_vector &v1, t_vector &v2, t_vector *result)
{
  result->Init(v2);
  result->Sub(&v1);
  result->Mult(0.5);
  result->Add(&v1); 

//  result->x = v1.x + ((v2.x - v1.x) / 2.0); 
//  result->y = v1.y + ((v2.y - v1.y) / 2.0); 
} 


/*
void turn_xy(t_vertex_2d *P,t_vertex_2d Around,double angle)
{ // around ist der Drehpunkt
  // angle ist der Winkel im Bogenmass um den der Punkt P gedreht wird
  t_vertex_2d delta;
  double sin_a,cos_a;    // sin(a) & cos(a)

#ifdef DEBUG
  fprintf(stderr,"p=(%g,%g), center=(%g,%g), angle=%g\n",
                     P->x,P->y, Around.x,Around.y, angle); 
#endif    

  delta.x = P->x - Around.x;
  delta.y = P->y - Around.y;
  
  sin_a = sin(angle);
  cos_a = cos(angle);
  
  P->x = Around.x + ((delta.x*cos_a) - (delta.y*sin_a)); 
  P->y = Around.y + ((delta.x*sin_a) + (delta.y*cos_a)); 
}
*/

/*
void norm_vec(t_vertex_2d *P)
{
  double nx,len;
  
  if ((P->x == 0) && (P->y == 0))
     return;
  
  len = sqrt(SQR(P->x) + SQR(P->y));
  P->x /= len;
  P->y /= len;
  
}
*/

// helper function for SolveCoords
void Len2_Polar(int i, int j, double *dist, double *angle)
{
   double l,r,d,u,delta;

#ifdef DEBUG
    fprintf(stderr,"trying to solve (%d,%d)",i,j);
#endif

    l = (double)i;
    l *= LEFT_tick2mm;
    
    r = (double)j;
    r *= RIGHT_tick2mm; 

    if (i==0)
       {
        *dist = - DIAMETER / 2.0;
        *angle = r / DIAMETER;
#ifdef DEBUG        
        fprintf(stderr,"links %d  ",j);
#endif
        return;
       }
       
    if (j==0)
       {
        *dist = DIAMETER / 2.0;
        *angle = - l / DIAMETER;
#ifdef DEBUG
        fprintf(stderr,"rechts %d  ",i);
#endif
        return;
       }

//    d = (DIAMETER / (1.0 - ((double)j/(double)i)  ));

    u = l - r;
    delta = (u / DIAMETER);
    d = (l/u) * DIAMETER;
   
    if ((i*j) > 0)
       {
        if (l > r)
           *dist = d - (DIAMETER/2.0); 
         else
           *dist = d + (DIAMETER/2.0); 
#ifdef DEBUG
        fprintf(stderr,"bogen %d,%d  dist=%g, u=%g\n",i,j,*dist,u);
#endif
        return;
       }  


    if (l > r)
       *dist = d - (DIAMETER/2.0); 
     else
       *dist = d + (DIAMETER/2.0); 
#ifdef DEBUG
    fprintf(stderr,"drehung %d,%d  dist=%g  ",i,j,*dist);
#endif
}



double Exp(double x, int prec)
{
	double X,F,S;

	F = 1.0;
        S = 1.0;
	X = x;
        for (int i=0; i<prec; i++)
          {
	   S += (X/F);
           F *= ((double)i+2.0);
	   X *= x;
	  }
        
	return S;
}


/*
void rotate(double &x, double &y, double rho)
{
	double X = (x*cos(rho)) + (y*sin(rho));
	double Y = (-x*sin(rho)) + (y*cos(rho));
	x = X;
	y = Y;
} 

void rotate(float &x, float &y, double rho)
{
	float X = (x*cos(rho)) + (y*sin(rho));
	float Y = (-x*sin(rho)) + (y*cos(rho));
	x = X;
	y = Y;
} 
*/








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