/* 
 * University of Leipzig: khepera.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) 
 * 
 */


// Output from p2c 1.21alpha-07.Dec.93, the Pascal-to-C translator
// From input file "khepera.p"

// #include <p2c/p2c.h>


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

#include "copyright.h"

/*
#ifdef hpux
#include "timer.h"
#endif
*/

#define K_EXTEND_G

#include "k_extend.h"
//#include "khep_serial.h"
#include "trigonom.h"
//#include "sim-socket.h"
//#include "world.h"
#include "fft.h"
#include "utils.h"
#include "timer.h"

int straight = 0;
int arc = 0;
int turn = 0;
/*
        double SensorVec[8] = { M_PI, 
                                (M_PI*3.0) / 4.0, 
                                (M_PI/2.0) + (M_PI/32.0), 
                                (M_PI/2.0) - (M_PI/32.0), 
                                M_PI / 4.0, 
                                0.0, 
                                -(M_PI/2.0) + (M_PI/32.0),                                 
                                -(M_PI/2.0) - (M_PI/32.0)  
                               };             

*/

//#ifdef DEBUG
int rct = 0;
//#endif



void t_robot_ext::Reset(void)
{

//#ifdef DEBUG
  rct += 1;
  if (rct == 10)
     {
      fprintf(stderr,"make an ugly error here for debugging (search me with grep)\n");
      fprintf(stderr,"%s\n",(char *)1);  // this pointer should point nowhere to get a fast breakpoint in 
                                 // gdb after 10 calls to Reset() 
//      exit(1);                           
     }  
//#endif
  t_robot_basic::Reset();
     
  Loc.P.x = 0.0;
  Loc.P.y = 0.0;
  Loc.aspect = 0.0;
  Loc.delta = 0.0;

  alpha = M_PI / 2.0;
  full_alpha = 0.0;
  d_alpha = 0.0;
  
  gpl = 0;
  gpr = 0;

  lgpl = 0;
  lgpr = 0;
  
  gone = 0.0;
  delta_s = 0.0;
  delta_s_avg->Reset();
 	
  odo_frame = 0;	
//  sensor_avg_width = 2.0;

  World.Init();
  Trajectory->Init(&Loc.P);
  MsgQueue.Init((char *)this);
}


void t_robot_ext::Init(char *s)
{
  // these things have to be done only once (not twice or more)
  if (FFT_initialized == false)
     {
      InitializeFFT(64);
//#ifdef hpux
      InitTimer();
//#endif      
     } 

	Trajectory = new t_PATH;
	delta_s_avg = new t_average;
	delta_s_avg->Init(100.0);	

	  t_robot_basic::Init(s);
//  t_robot_ext::Reset();
//        already virtually called by t_robot_basic::Init()
}


void t_robot_ext::Done(void)
{
	fprintf(stderr,"t_robot_ext::Done()\n");
	t_robot_basic::Done();
	
//	delta_s_avg->Done();
	delete delta_s_avg;
	delta_s_avg = NULL;

//	Trajectory->Save("log_Path.txt");
	Trajectory->Done();	
	
	if (FFT_initialized == true)
	   EndFFT(); 
	FFT_initialized = false;

	DoneTimer();   
}




// wait to finish a formerly started action
void t_robot_ext::WaitStopped(void)
{
  int arm,w,opl,opr;
 
  used = true;
  
  do {
      opl = gpl;
      opr = gpr;
      w = gr_width;
      arm = arm_pos;

      usleep(100000);   // 0.1 sec
 
      SolveCoords(false);
      GrippedWidth();
      GetArmPos();

#ifdef DEBUG    
      fprintf(stderr,"0x%x\n",GetMotionCtrlFlags() & AT_POSITION); 
      fprintf(stderr,"l:(%d==%d),r:(%d==%d) w:(%d==%d),p:(%d==%d)\n",
              gpl,opl,gpr,opr,gr_width,w,arm_pos,arm); 
#endif
      
     } while (
              (gpl != opl) || 
              (gpr != opr) || 
              (w != gr_width) ||
              (arm != arm_pos)
             );

//  do { 
//       usleep(50000); 
//     } while ((GetMotorSpeed(0) != 0) || (GetMotorSpeed(1) != 0));     
}


//~~~~~~~~~~~~~~~
// extra funcs
//_______________
void t_robot_ext::Stop(void)
{
  t_robot_basic::Stop();
   
  WaitStopped();
}

// turn the robot 
void t_robot_ext::Turn(double degree)
{
  Stop();
  SolveCoords(false);

  t_robot_basic::Turn(degree);

  WaitStopped();
  SolveCoords(false);
}

void t_robot_ext::LookHome(void)
{
  Turn(180 + DEG(Loc.aspect) - DEG(alpha));
}

// set robot to a given direction
void t_robot_ext::SetOrientation(double degree)
{
  Turn(degree - DEG(alpha));

//  alpha = RAD(degree);
#ifdef DEBUG
  fprintf(stderr,"alpha:%g\n",DEG(alpha));
#endif  
}

// move robot 'mm' millimeter forward
void t_robot_ext::Move(double mm)
{
  Stop();				// stop the motors 		
  SolveCoords(false);			// check position
    
  t_robot_basic::Move(mm); 

  WaitStopped(); 
  SolveCoords(false); 
}

// calculate the new X,Y position from the wheel counters


void t_robot_ext::SolveCoords(char nonblock_io)
{
    int i,j/*,L,R*/;
 	

    if (nonblock_io == true)
        MsgQueue.request_wheelposition();	
     else
        ReadWheelCounter(&gpl,&gpr);

    i = gpl - lgpl;
    j = gpr - lgpr;

//    fresh_odo++;	

    delta_s = 0.0;
    d_alpha = 0.0;

    if ((i==0) && (j==0))
       return;  // nothing happened

//    fresh_odo = 0;
    odo_frame++; 	
	   
    lgpl = gpl;
    lgpr = gpr;	

    motion_flags &= !AT_HOME;
    used = true;
	
    if (i == j)
       { // straight forward/backward
	delta_s = ((double)i) * tick2mm;
	delta_s_avg->Add(delta_s);

        double dx = (cos(alpha) * delta_s);
        double dy = (sin(alpha) * delta_s);
     
        Loc.P.x += dx;
        Loc.P.y += dy;
       } 	
     else
       {
	double u = (double)(j - i) * tick2mm;	// delta alpha
	d_alpha = RAD_NORM(u / DIAMETER);

        u = (double)(gpr - gpl);
	full_alpha = u * tick2mm / DIAMETER;  // totale Verdrehung seit Beginn
        u = RAD_NORM(full_alpha);

        alpha = RAD_NORM(u + (M_PI/2.0)); 
			// alpha zeigt auf der Karte nach Norden
			// deshalb 90° nach links gedreht
        if (i == -j)
	   { // only turn, no position change
	   }
	 else
	   { // arc 
 	    double w = ((double)(i+j)*tick2mm) / 2.0;

// 	    double da2 = alpha - (d_alpha / 2.0); 
//          double dx = w * cos(da2);
//          double dy = w * sin(da2);

	    double RR = w / d_alpha;
	    
	    double dx = cos((M_PI/2.0) + alpha - d_alpha) + cos(alpha - (M_PI/2.0));
	    double dy = sin((M_PI/2.0) + alpha - d_alpha) + sin(alpha - (M_PI/2.0));
	    dx *= RR;
	    dy *= RR;	

	
            Loc.P.x += dx;      
            Loc.P.y += dy;
#ifdef DEBUG
	    delta_s = sqrt(SQR(dx) + SQR(dy));
            double error = (fabs(delta_s) - fabs(w)) *50.0/ (fabs(delta_s) + fabs(w));
	    if (fabs(error) > 1.0)
		fprintf(stderr,"diff: %g%%\n",error);
#endif
	    delta_s = w;
	    delta_s_avg->Add(delta_s);
	   }
       }

// #ifdef WITH_POLAR_COORDS
    Loc.aspect = clip_angle(atan2(-Loc.P.y,-Loc.P.x));   
    Loc.delta = sqrt(SQR(Loc.P.x) + SQR(Loc.P.y));
// #endif
    gone += delta_s;         

    World.Update(Loc.P);
    Trajectory = Trajectory->Add(&Loc.P);	
}

/*
void t_robot_ext::__SolveCoords(void)
{
    int L,R,i,j;
    double l,r,u,d,x,beta,dx,dy,alpha_old;
    double an,ao;


#ifdef VERY_CAREFUL_ROB
    sprintf(st,"E");  // read current MotorSpeed
    serial_talk(fd,(char *)&st);
    
    if (sscanf(st,"e,%d,%d",&i,&j) == 2)
       {
        Stop();    		     // have a short break	
        ReadWheelCounter(&L,&R);
        SetWheelCounter(0,0);
        gpl = 0;
        gpr = 0;
        sprintf(st,"D,%d,%d",i,j);   // turn Motors on
        serial_talk(fd,(char *)&st);
       }
      else
#endif
        ReadWheelCounter(&L,&R);
        
                
    i = L - gpl;
    j = R - gpr;
    
    if ((i==0) && (j==0))
       return;  // nothing happened

    motion_flags &= !AT_HOME;
   
    alpha_old = alpha;

    gpl = L;
    gpr = R;   

    u = (double)(gpr - gpl);

    if (u > 0.0)
        alpha = u * RIGHT_tick2mm;
     else
        alpha = u * LEFT_tick2mm;
        
    alpha /= DIAMETER;  

    alpha += (M_PI/2.0);

    an = alpha - alpha_old;
#ifdef DEBUG    
    fprintf(stderr,"alpha=%g° da=%g°  ",DEG(alpha),DEG(an));
#endif

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

    u = l - r;

    if ((i != j) && (fabs(u) > 0.5))
       { // turn or arc
         if (i != -j)
            { // arc

             arc++;      
             // rad
//             alpha = (gpl - gpr) * TICK_2_MM / DIAMETER;
//             alpha = clip_angle(alpha + a); 
            }
           else
            { // only turn
//             alpha -= (i * TICK_2_MM * 2) / DIAMETER;
             turn++;  
            }  
       }
     else
       { // nearly straight
        
        //d = (((double)(i + j)) * tick2mm) / 2.0;      
        
        double delta = atan2((l-r),DIAMETER);
//        alpha += delta;
     
        straight++; 
#ifdef DEBUG       
        fprintf(stderr,"delta=%g°,d=%g ",DEG(delta),d);
#endif
       }  


#ifdef DEBUG
    fprintf(stderr,"X=%g, Y=%g, i=%d, j=%d, alpha=%g°, aspect=%g°\n",
               pos.x,pos.y,i,j,DEG(alpha),DEG(aspect));   
#endif
}
*/
/*
double t_robot_ext::ExtraSensorAvg(int stype,int Num)
{
  // call UpdateSensors() first 
	return Sensor[stype % 2][Num % 16].extra;
}


double t_robot_ext::ReadSensorAvg(int stype,int Num)
{
  // call UpdateSensors() first 
  double d;
  
  d =  (double)Sensor[stype % 2][Num % 8].AvgV;
//  d /= ((double)Sensor[stype % 2][Num % 8].Max);

  return d;      
}    
*/

double error_diff(double loc,double gwidth,double value)
{
    return gaussian(RAD_NORM(loc),gwidth,1.0) * value;
}
 
 
// return true wenn Sensor[i] "gleichverteilt" ist

double t_robot_ext::Kompass(int sens_type ,double &reliable)
{
	if (sens_type == AMBL)
	   MsgQueue.request_ambient();
	 else 
	   MsgQueue.request_proximity();


//#define SNCN 8
//	int w_idx[SNCN] = {5,4,3,2,1,0,7,6}; 
#define SNCN 16
	int w_idx[16] = {5,12,4,11,3,10,2,9,1,8,0,15,7,14,6,13}; 

	t_SensorGroup *SensorGrp;

	if (sens_type == IRED)
	   SensorGrp = _Ired_;
         else
           SensorGrp = _Ambl_;

	reliable = SensorGrp->Information();  

	double range = SensorGrp->GetRange();

	if (range <= 0.0001)
	   { 
#ifdef DEBUG
	    fprintf(stderr,"t_tobot_ext::Kompass() range:%g, max:%g, min:%g\n"
	                     ,range,SensorGrp->Range.max,SensorGrp->Range.min);	
#endif
	    return sigma;
	   } 

	double T,B,OW;
	T = 0.0;
	B = 0.0;   	
	OW = SensorGrp->Sensor[w_idx[SensorGrp->min_idx]]->Loc;

	for (int j=0; j<16; j++)
	  {
	   int i = (SensorGrp->min_idx + j) % 16;

	   if (DistSensor(i) == false)
	      continue;

	   double w = SensorGrp->Sensor[w_idx[i]]->Loc; 	
	   while (w < OW)
	      w += (M_PI * 2.0);

	   OW = w;
	   double v = SensorGrp->Sensor[w_idx[i]]->Value;	    

	   T += (w * v);
	   B += v;
	  }	 

        if (fabs(B) != 0.0)
	    sigma = RAD_NORM((-T/B));	
#ifdef DEBUG
	 else
	    fprintf(stderr,"t_tobot_ext::Kompass() B == 0.0\n");
#endif
	    
	return sigma;	
}

double t_robot_ext::Kompass_old(int sens_type, char reverse, double &gwidth, double &reli)
{
   	double EV[16];	

	t_SensorGroup *SensGrp;

	if (sens_type == IRED)
	   SensGrp = _Ired_;
         else
	   SensGrp = _Ambl_;  	

//	min_v = SensGrp->Range.min;
//   	max_v = SensGrp->Range.max;

	for (int i=0; i<16; i++)
	   if (reverse == false)
	      EV[i] = SensGrp->Sensor[i]->AvgV;	
	    else
	      EV[i] = 1.0 - SensGrp->Sensor[i]->AvgV;	

	if (SensGrp->GetRange() <= 0.001)
	   {
#ifdef DEBUG
	    fprintf(stderr,"very unreliable sensor values\n");
#endif            
	    return (M_PI/2.0);
	   } 

	reli = SensGrp->Information();
  

   // erster Durchlauf, auf 10° genau schätzen
	double max_err, min_j, min_k;
	double min_v = SensGrp->Range.min;
	max_err = 0.0;
	min_j = 0.0;

	for (int j=-18; j<18; j++)
	  {
	   double err = 0.0;   	      
	   for (int i=0; i<16; i++)
             {
     	      if (DistSensor(i) == false)
                 continue;

              double m = (double)j*M_PI/18.0;
              min_v = m;
              m += SensGrp->Sensor[i]->Loc;

   	      err += error_diff(m,gwidth,EV[i]);
	     }

	   if (err > max_err)
              {
               max_err = err;
	       min_j = min_v;
 	      }    	 
	  } 	

     
   // zweiter Durchlauf, auf 1.1° genau schätzen 
	max_err = 0.0;
	min_k = 0.0;

	for (int j=-18; j<18; j++)
	  {
	   double err = 0.0;   	      
	   for (int i=0; i<16; i++)
             {
     	      if (DistSensor(i) == false)
                 continue;

              double m = min_j;
              m += (double)j*RAD(11.0)/18.0;  
              min_v = m;

	      m += SensGrp->Sensor[i]->Loc;

   	      err += error_diff(m,gwidth,EV[i]);
	     }

	   if (err > max_err)
              {
	       max_err = err;
	       min_k = min_v;
	      }    	 
	  } 	
     
   // 3. Durchlauf, in 32 Stufen Breite der besten Gaussglocke schätzen
	max_err = 1000000.0;
	double min_g = gwidth;

	for (int j=48; j>0; j--)
	  {
	   double err = 0.0;   	      
	   double v = (double)j / 20.0;
	   v += 0.4;

	   for (int i=0; i<16; i++)
             {
	      if (DistSensor(i) == false)
        	 continue;

              double m = min_k;
	      m += SensGrp->Sensor[i]->Loc;

    	      double g = gaussian(RAD_NORM(m),v,1.0);

              err += fabs(g - EV[i]);
	     }

	   if (err < max_err)
              {
	       max_err = err;
	       min_g = v;
	      }    	 
	  } 	


	if (gwidth > min_g)
      	   gwidth -= 0.002;
	 else
	   if (gwidth < min_g)
	      gwidth += 0.006;

//   gwidth = min_g;          

  min_k = RAD_NORM(min_k);
  min_j = RAD_NORM(min_j);

#ifdef DEBUG  
//  if (reverse == false)
//    
fprintf(stderr,"min_j:%g°\tmin_k:%g°\tmax_err:%g\n",DEG(min_j),DEG(min_k),max_err);
#endif
 
  if (reverse == true)
     return DEG_NORM(DEG(min_k)); 
   
  return DEG_NORM(DEG(min_k)+180.0);   
}



void t_robot_ext::SetMotorSpeed(double _left, double _right)
{
//  char st[1024];

  set_speed_left = (int)(_left * MAX_SPEED);
  set_speed_right = (int)(_right * MAX_SPEED);

  MsgQueue.send_motorspeed(set_speed_left,set_speed_right);
}


void t_robot_ext::SetMotorSpeed(int MotorNum, double Speed)
{
//    int i,j; 
//    char st[128];

    if (MotorNum == 0)	// set the appropriate speed value
        set_speed_left = (int)(Speed * MAX_SPEED);
     else
        set_speed_right = (int)(Speed * MAX_SPEED);
             
    MsgQueue.send_motorspeed(set_speed_left,set_speed_right);
}



//~~~~~~~~~~~~~~~~~~~~~~~~~~
// VISION-Turret
//__________________________


void swap(short int &A, short int &B)
{
   short int C;
   
   C = A;
   A = B;
   B = C;
}




/* 
char t_robot_ext::ImageRead(void)
{
   t_robot_basic::ImageRead(); 

   if (With_Camera == true)
      {
       HighPass(&Image[0],64,64);
       Clipping(&Image[64],64,0.16);
       
       for (int j=0; j<64; j++)
         Image[192+j] = Image[j] << 5;
         
       RealFFT(&Image[192]);


#ifdef DEBUG
       fprintf(stderr,"swap()\n"); 
#endif    
  // 128..191 FFT        
       for (int j=0; j<32; j++)  
         {
          int k = (j * 2) + 128; 
          int n = BitReversed[j] + 192;

          double d = SQR(Image[n]) + SQR(Image[n+1]);
          if (d > 0.0)
             Image[k] = (int)(sqrt(d));
           else
             Image[k] = 0;   

          Image[k+1] = Image[k];      
         } 
         
       Image[128] = 0;  
       Image[129] = 0;  
       Clipping(&Image[128],64,0.16);  

  // 192..255 Highpass of FFT
       HighPass(&Image[128],64,192);
       Clipping(&Image[192],64,0.08);  
      }
        
   return With_Camera;        
}
*/


// get the number of the pixel with the highest gradient
int t_robot_ext::GetVisualEdge(void)
{
  // your task -- not hardware supported
  return 0;
}



// End.







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