/* 
 * 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 <math.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>



#include "copyright.h"

/*
#if (hpux || SPIKE_DETECT)
#include "timer.h"
#endif
*/

#include "khep_serial.h"
#include "t_msgtype.h"

#define K_BASIC_G

#include "k_basic.h"
#include "trigonom.h"
#include "netutils.h"
//#include "sim-socket.h"

        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)  
                               };             


	double SensorVec16[16];



void mk_sensorVec16(void)
{
	for (int i=0; i<8; i++)
	  {
	   SensorVec16[i] = SensorVec[i];
	   
	   if (i==7)
	      SensorVec16[i+8] = (-M_PI + SensorVec[i]) / 2.0;
	    else	
	      SensorVec16[i+8] = (SensorVec[i] + SensorVec[i+1]) / 2.0;
	  }	
}


char t_robot_basic::DistSensor(int i)
{
/*	a Dist(tributed) Sensor is evenly spaced and might
        be a virtual one, virtual means interpolated in between 
	two real sensors 
*/
	 switch (i) 		//           2  10. 3
	   {			//        9	      11
	    case 2:		//      1.		4.
	    case 3:		//
	    case 6:		//    8 		 12
	    case 7:		//
	    case 8:		//    0.		  5.
	    case 9:		//
	    case 11:		//
	    case 12:		//      15.	       13.
	      return false;	//
	      break;		//           7 14. 6
	   }
           
   return true;        
}

void t_robot_basic::Reset(void)
{
#ifdef DEBUG
  fprintf(stderr,"t_robot_basic::Reset()\n");
#endif

  catched = 0;
//  dark = 0;
  motion_flags = 0;

  Ired->Reset(0.5, 3, SensorVec);
  Ambl->Reset(0.5, 3, SensorVec);	 	

  _Ired_->Reset(0.5, 3, SensorVec16);
  _Ambl_->Reset(0.5, 3, SensorVec16);	 	
  
  With_Camera = true;       //  assume to have these things
  With_Gripper = true;      //   -- disabled later on command failure 
  image_quality = 0.0;

  SetWheelCounter(0, 0);
  
  gr_width = GrippedWidth();
  arm_pos = GetArmPos();
  ohm = ReadGripOhmMeter();
  gr_update_flag = 0;	

  Release();
  Lift();
  LED_Ctrl(0, ON);
  LED_Ctrl(1, ON);
  
  if (ImageRead() == true)
     SetScanningPeriod(1);	// fastest scan
//   else
//     With_Camera = false;  

  used	= FALSE;		// mark as "in initial state"
}

void haltfunction(void)
{
	fprintf(stderr,"halt\n");
}

void t_robot_basic::Init(char *s)
{
  char st[1024];
  int p;

#ifdef DEBUG  
  fprintf(stderr,"t_robot_basic::Init()\n");
#endif
  strcpy(device, s);

  mk_sensorVec16();	
  
  if (sscanf(s,"%s %d",&st[0],&p) == 2)
     {
      _FD_ = clConnect_to(st,p);
      socket_mode = 1;
     }
   else  
     {
      _FD_ = serial_open(device);
      socket_mode = 0;
     } 

  if (_FD_ <= 0)
     {
       fprintf(stderr,"could not open file: %s\n",device);
       exit(1);
     }

#ifdef DEBUG
  fprintf(stderr,"device:[%s]\n",device);
#endif
  Ired = new t_SensorGroup;
  Ambl = new t_SensorGroup;
  _Ired_ = new t_SensorGroup;
  _Ambl_ = new t_SensorGroup;
	 
  Ired->Init(8, 0.5, 3, SensorVec);
  Ambl->Init(8, 0.5, 3, SensorVec);

  _Ired_->Init(16, 0.5, 3, SensorVec16);
  _Ambl_->Init(16, 0.5, 3, SensorVec16);

//  fprintf(stderr,"t_robot_basic:: SensorGroup done\n");
#ifdef DEBUG
  haltfunction();	
#endif
  Reset();
}


void t_robot_basic::Done(void)
{
	fprintf(stderr,"t_robot_basic::Done()\n");
  if (socket_mode == 1)
     ShutDown(_FD_);
   else  
     close(_FD_);
}


//~~~~~~~~~~~~~~~
// extra funcs
//_______________
void t_robot_basic::Stop(void)
{
  used = TRUE;

  t_robot_basic::SetMotorSpeed(0,0);
}

// turn the robot 
void t_robot_basic::Turn(double degree)
{
  int cnt_left,cnt_right;

  while  (degree>180.0)
     degree -= 180.0;
  
  while (degree<-180.0)
     degree += 180.0;   

#ifdef DEBUG  
  fprintf(stderr,"deg:%g\n",degree);     
#endif
  ReadWheelCounter(&cnt_left, &cnt_right);
  
  int a = (int)(6 * RAD(degree) * DIAMETER);
   
  cnt_left -= a;
  cnt_right += a;
  TurnWheels(cnt_left, cnt_right);
}



// move robot 'mm' millimeter forward
void t_robot_basic::Move(double mm)
{
  int cnt_left,cnt_right;
  
  ReadWheelCounter(&cnt_left,&cnt_right);	 
  int a = long(mm *12);
  cnt_left += a;			// calculate motor pulses	 
  cnt_right += a;
  TurnWheels(cnt_left, cnt_right);		
}




//~~~~~~~~~~~~~~~~~~~
// BASIC-Turret
//___________________
void t_robot_basic::ReadBiosVersion(char *s, const int MAX_s)
{
}


void t_robot_basic::ReadAddInfo(InfoType itype, void *string)
{
}


void t_robot_basic::SetWheelCounter(int left, int right)
{
  char st[1024];

  *st = '\0';
  sprintf(st,"G,%d,%d",left,right);
  serial_talk(_FD_,st);

  used = TRUE;
}


void t_robot_basic::TurnWheels(int left, int right)
{
  char st[1024];

  if ((left != 0) || (right != 0))
      motion_flags &= ~AT_HOME;		// not at home any longer
 
  sprintf(st,"C,%d,%d",left,right);
  serial_talk(_FD_,st);

  used = TRUE;
}


void t_robot_basic::ReadWheelCounter(int *left, int *right)
{
    char st[1024];

    sprintf(st,"H");
    serial_talk(_FD_,(char *)&st);
#ifdef DEBUG
    fprintf(stderr,"read wheel cnt: %s\n",st);
#endif        
    sscanf(st,"h,%d,%d",left,right);

    used = TRUE;
}



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

  speed_left = (int)(_left * MAX_SPEED);
  speed_right = (int)(_right * MAX_SPEED);

  SetMotorSpeed(speed_left,speed_right);
}

void t_robot_basic::SetMotorSpeed(int _left, int _right)
{
  char st[1024];

  speed_left = _left;
  speed_right = _right;

  if ((speed_left != 0) || (speed_right != 0))
      motion_flags &= ~AT_HOME;

//  SolveCoords(); 
  fprintf(stderr,"t_robot_basic:: SetMotorSpeed(%d,%d)\n",_left,_right);

  sprintf(st,"D,%d,%d",speed_left,speed_right);
  serial_talk(_FD_,st);

  used = TRUE;
}


double t_robot_basic::GetMotorSpeed(int MotorNum)
{
    int i,j; 
//    char st[32];

    i = 0;
    j = 0;

    GetMotorSpeed(i,j);

    if (MotorNum == 0)
        return (double)(i / MAX_SPEED);
     else
        return (double)(j / MAX_SPEED); 
       
    return 0;
}

void t_robot_basic::GetMotorSpeed(int &L, int &R)
{
    int i,j; 
    char st[32];

    used = TRUE;

    sprintf(st,"E");
    serial_talk(_FD_,(char *)&st);
    
    if (sscanf(st,"e,%d,%d",&i,&j) == 2)
       {
        L = i;
        R = j;
       }
}


// reads the sensors
char getcode_str[4][5] = {"N\0","O\0","n\0","o\0"}; 



void t_robot_basic::UpdateSensors(void)
{
    	char st[2048];

    	used = TRUE;

    	for (int i=0; i<2; i++)
	  {
	   sprintf(st,getcode_str[i]);
    	   serial_talk(_FD_,(char *)&st);
	   __ReadLEDs(i,st);
          }

}



void t_robot_basic::SetSpeedProfile(int MaxLeft, int MaxRight, int AccLeft,
				   int AccRight)
{
  char st[1024];

  used = TRUE;
  sprintf(st,"J,%d,%d,%d,%d", MaxLeft, AccLeft, MaxRight, AccRight);
  serial_talk(_FD_,st);
}


int t_robot_basic::GetMotionCtrlFlags(void)
{
    used = TRUE;
    int i;
    char st[1024];
    sprintf(st,"R,32");
    serial_talk(_FD_,st);

    if (sscanf(st,"r,%d",&i) == 1)
       {
        if (i & 1)
	   motion_flags |= AT_HOME;
         else
	   motion_flags &= ~AT_HOME;
       }

    sprintf(st,"K");
    serial_talk(_FD_,st);
	
    __GetMotionCtrlFlags(st);	

    return motion_flags;	
}





// do something with the lights
void t_robot_basic::LED_Ctrl(int LED_Num, LED_Behavior behave)
{
  char st[1024];

  used = TRUE;

  switch (behave)
    {
     case ON:  sprintf(st,"L,%d,1",LED_Num);
       break;

     case OFF: sprintf(st,"L,%d,0",LED_Num);
       break;

     case CHANGE: sprintf(st,"L,%d,2",LED_Num);
       break;
    }       
  
  serial_talk(_FD_,st);
}



//~~~~~~~~~~~~~~~~~~
// GRIP-Turret
//__________________

// catch the object 
void t_robot_basic::Grip(void)
{
  char st[1024];

  if (With_Gripper == false)
     return;	 

  used = TRUE;
  sprintf(st,"T,1,D,1");
  serial_talk(_FD_,st);
}

// get it off the ground
void t_robot_basic::Lift(void)
{
  char st[1024];

  if (With_Gripper == false)
     return;	 

  used = TRUE;
  sprintf(st,"T,1,E,%d",140);
  serial_talk(_FD_,st);
}

// drop it
void t_robot_basic::Release(void)
{
  char st[1024];

  if (With_Gripper == false)
     return;	 

  used = TRUE;
  sprintf(st,"T,1,D,0");
  serial_talk(_FD_,st);
}

// swirl your arm ;-)
void t_robot_basic::SetArmPos(int degree)
{
  char st[1024];
  int j = 128;

  if (With_Gripper == false)
     return;	 

  used = TRUE;

  if (degree >= -45 && degree <= 90)
    j = (int) (128 + (90 - degree) * 1.1777777777777);  // correction 1
  else if (degree >= 91 && degree <= 225)
    j = (int) (30 + (180 - degree) * 1.0888888888888);  // correction 2

  if (j > 255)  // clip 
    j = 255;

  if (j < 0)
    j = 0;

  sprintf(st,"T,1,E,%d",j);
  serial_talk(_FD_,st);
}

// decide between copper and wood -- nothing more
int t_robot_basic::ReadGripOhmMeter(void)
{
    int i; 
    char st[32];

    used = TRUE;
  
    if (With_Gripper == false)
       return 0;	 

    sprintf(st,"T,1,F");
    serial_talk(_FD_,(char *)&st);
    
    if (sscanf(st,"t,1,f,%d",&i) == 1)
       {
	ohm = i;
	With_Gripper = 1;        
       }	
     else
	With_Gripper = 0;        

    return ohm;
}

// there is an object between the gripper
// boolean 
char t_robot_basic::CatchedObject(void)
{
    char st[1024];
    int i;

    if (With_Gripper == false)
       return 0;	 

    used = TRUE;
  
    sprintf(st,"T,1,G");
    serial_talk(_FD_,(char *)&st);
    
    if (sscanf(st,"t,1,g,%d",&i) == 1)
       {
        if (i == 255)
           catched = true;
          else
           catched = false;

	With_Gripper = 1;        
       }	
     else
	With_Gripper = 0;        

#ifdef DEBUG 
    fprintf(stderr,"catched:%d %d\n",catched,i);
#endif
    
    return catched;
}


// measure the object (20..200)
// byte
int t_robot_basic::GrippedWidth(void)
{
   char st[1024];
//   int i;

  if (With_Gripper == false)
     return 0;	 

   used = TRUE;
  
   sprintf(st,"T,1,H,0");
   serial_talk(_FD_,(char *)&st);
    
   __GripperRead(st);

   return gr_width;
}


// where are my arms ;-)
int t_robot_basic::GetArmPos(void)
{
   char st[1024];
   int i;

  if (With_Gripper == false)
     return 0;	 

   used = TRUE;
  
   sprintf(st,"T,1,H,1");
   serial_talk(_FD_,(char *)&st);
    
   i = gr_width;
   if (__GripperRead(st) == false)
      {
       arm_pos = gr_width;	
       gr_width = i;
      }		  

   return arm_pos; 	
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~
// VISION-Turret
//__________________________
char t_robot_basic::ImageRead(void)
{
   char st[1024];
//   int i;

   if (With_Camera == false)
      return 0;	 

   used = TRUE;
  
   sprintf(st,"T,2,N");
   serial_talk(_FD_,(char *)&st[0]);
    
   __ImageRead(st);

   return With_Camera;        
}


// get the avergage light intensity
double t_robot_basic::GetIntensity(void)
{
   char st[1024];

   if (With_Camera == false)
      return 0.0;	 

   used = TRUE;
  
   sprintf(st,"T,2,L");
   serial_talk(_FD_,(char *)&st);

   __GetIntensity(st);

   return Iris.Value;	
}


// get the number of the pixel with the highest intensity
// (-1) if failed
int t_robot_basic::GetPixelMaxLight(void)
{
   char st[1024];
   int i;

   if (With_Camera == false)
      return 0;	 

   used = TRUE;
  
   sprintf(st,"T,2,O");
   serial_talk(_FD_,(char *)&st);
    
   if (sscanf(st,"t,2,o,%d",&i)==1)
       return  i;
     else
       return -1;  
}


// get the number of the pixel with the lowest intensity
// (-1) if failed
int t_robot_basic::GetPixelMinLight(void)
{
   char st[1024];
   int i;

   if (With_Camera == false)
      return 0;	 

   used = TRUE;
  
   sprintf(st,"T,2,P");
   serial_talk(_FD_,(char *)&st);
    
   if (sscanf(st,"t,2,p,%d",&i)==1)
       return  i;
     else
       return -1;  
}



void t_robot_basic::SetScanningPeriod(int period)
{
   char st[1024];

   if (With_Camera == false)
      return;	 

   used = TRUE;
  
   sprintf(st,"T,2,U,%d",period);
   serial_talk(_FD_,(char *)&st);
}




// End.




//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// new string parsing functions
char t_robot_basic::__GetMotorSpeed(char *st)
{
    int i,j;	
    char good = false;	

    if (sscanf(st,"e,%d,%d",&i,&j) == 2)
       {
        speed_left = i;
        speed_right = j;
	good = true;
       }

    return good;	
}

char t_robot_basic::__ReadLEDs(int code, char *st2)
{
    int Value[8];
    double rv[8],qv[16];	
    char format[256]; 
    char good = false;	

    strcpy(format,getcode_str[code+2]);    
    strcat(format,",%d,%d,%d,%d,%d,%d,%d,%d");

    if (sscanf(st2,format,
        &(Value[0]),&(Value[1]),&(Value[2]),&(Value[3]),
        &(Value[4]),&(Value[5]),&(Value[6]),&(Value[7])) == 8)
       { 
	good = true;
	
	t_SensorGroup *SnsGrp,*SnsGrp__;

	if (code == IRED)
	   { 
	    SnsGrp = Ired;	
	    SnsGrp__ = _Ired_;	

	    for (int i=0; i<8; i++)
	       rv[i] = (double)Value[i];
	   }
	 else
           { 
	    SnsGrp = Ambl;	
	    SnsGrp__ = _Ambl_;	

	    for (int i=0; i<8; i++)
	       rv[i] = 512.0 - (double)Value[i];
	   }

//	fprintf(stderr,"\n\n\n^^^^^^^^^ - %d -\n",code);
	SnsGrp->Update(rv);

	for (int i=0; i<8; i++)
	  {
	   int j = (i+1) % 8;
	   qv[i] = SnsGrp->Sensor[i]->AvgV;
//	   qv[i+8] = sqrt(qv[i] * SnsGrp->Sensor[j].AvgV);	// interpolate
	   qv[i+8] = (qv[i] + SnsGrp->Sensor[j]->AvgV)/2.0;	// interpolate
	  } 

//	fprintf(stderr,"\nvvvvvvvvvvv\n");
        SnsGrp__->Update(qv);
       }  

	return good;
}

char t_robot_basic::__ReadWheelCounter(char *st)
{
	long l,r;    	
	char good = false;

	if (sscanf(st,"h,%ld,%ld",&l,&r) == 2)
	   {
	    gpl = l;
	    gpr = r;	
	    good = true; 	
	   }	   

	return good;
}

char t_robot_basic::__GetMotionCtrlFlags(char *st)
{
   int T_left, M_left, E_left, T_right, M_right, E_right;
	char good = false;
//   int i;
//   motion_flags =0;

  if (sscanf(st,"k,%d,%d,%d,%d,%d,%d", &T_left, &M_left, &E_left, &T_right, &M_right, &E_right) == 6)
     {
	good = true;

      if (T_left)
	motion_flags |= AT_POSITION_LEFT;
	else
	motion_flags &= ~AT_POSITION_LEFT;

      if (M_left)
	motion_flags |= SPEED_DISPLACED_LEFT;
	else
	motion_flags &= ~SPEED_DISPLACED_LEFT;

      if (E_left)
	motion_flags |= POS_ERROR_LEFT;
	else
	motion_flags &= ~POS_ERROR_LEFT;

      if (T_right)
	motion_flags |= AT_POSITION_RIGHT;
	else
	motion_flags &= ~AT_POSITION_RIGHT;

      if (M_right)
	motion_flags |= SPEED_DISPLACED_RIGHT;
	else
	motion_flags &= ~SPEED_DISPLACED_RIGHT;

      if (E_right)
	motion_flags |= POS_ERROR_RIGHT;
	else
	motion_flags &= ~POS_ERROR_RIGHT;
    }

	return good;
}


char t_robot_basic::__GetPixelMaxLight(char *st)
{
   int i;
//   char good = false;

   if (sscanf(st,"t,2,o,%d",&i)==1)
	maxlight_idx =  i;
	else 
	return false;

   return true;
}

char t_robot_basic::__GetPixelMinLight(char *st)
{
   int i;
    
   if (sscanf(st,"t,2,p,%d",&i)==1)
       minlight_idx = i;
	else 
	return false;

   return true;
}


char t_robot_basic::__GetIntensity(char *st)
{
   int i,j,dark;
   char good = false;	

   if (sscanf(st,"t,2,l,%d,%d",&i,&j)==2)
      {
	dark = ((i << 8) + j);
	good = true;

	
	Iris.Set(log((double)dark+1.0) / 11.09034);
      }

   return good;
}

char t_robot_basic::__ImageRead(char *st)
{
	char gotcha = false;
	int iq = sscanf(st,"t,2,n,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
                      ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
                      ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
                      ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd",
      &Image[ 0],&Image[ 1],&Image[ 2],&Image[ 3],&Image[ 4],&Image[ 5],&Image[ 6],&Image[ 7],
      &Image[ 8],&Image[ 9],&Image[10],&Image[11],&Image[12],&Image[13],&Image[14],&Image[15],
      &Image[16],&Image[17],&Image[18],&Image[19],&Image[20],&Image[21],&Image[22],&Image[23],
      &Image[24],&Image[25],&Image[26],&Image[27],&Image[28],&Image[29],&Image[30],&Image[31],
      &Image[32],&Image[33],&Image[34],&Image[35],&Image[36],&Image[37],&Image[38],&Image[39],
      &Image[40],&Image[41],&Image[42],&Image[43],&Image[44],&Image[45],&Image[46],&Image[47],
      &Image[48],&Image[49],&Image[50],&Image[51],&Image[52],&Image[53],&Image[54],&Image[55],
      &Image[56],&Image[57],&Image[58],&Image[59],&Image[60],&Image[61],&Image[62],&Image[63]
      
        );

	image_quality = (double)iq / 64.0;

	if (iq == 64)
           {
	    gotcha = true;
	    image_quality += 1.0;	
	   }

	if (strncmp(st,"t,2,",4) == 0)
	   With_Camera = true;
	 else
	   With_Camera = false;	 			

   return gotcha;
}

char t_robot_basic::__ImageRead2(char *st)
{
	char gotcha = false;

   	int iq = sscanf(st,"t,2,s,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
                      ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd",
      &Image[ 0],&Image[ 2],&Image[ 4],&Image[ 6],
      &Image[ 8],&Image[10],&Image[12],&Image[14],
      &Image[16],&Image[18],&Image[20],&Image[22],
      &Image[24],&Image[26],&Image[28],&Image[30],
      &Image[32],&Image[34],&Image[36],&Image[38],
      &Image[40],&Image[42],&Image[44],&Image[46],
      &Image[48],&Image[50],&Image[52],&Image[54],
      &Image[56],&Image[58],&Image[60],&Image[62]);

	image_quality = (double)iq / 32.0;

	if (iq == 32)
       {
	for (int i=0; i<62; i+=2)
	  {
           int j = (i+2);
	   Image[i+1] = (Image[i]+Image[j]) / 2;
	  }

	Image[63] = Image[62];
	gotcha = true;
	image_quality += 1.0;	
       }
//     else  
//        With_Camera = false;

   return gotcha;
}

char t_robot_basic::__ImageRead4(char *st)
{
	char gotcha = false;

   	int iq = sscanf(st,"t,2,t,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
	              ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd",
	      	&Image[ 0],&Image[ 4],&Image[ 8],&Image[12],
	      	&Image[16],&Image[20],&Image[24],&Image[28],
		&Image[32],&Image[36],&Image[40],&Image[44],
		&Image[48],&Image[52],&Image[56],&Image[60]);

	image_quality = (double)iq / 16.0;

	if (iq == 16)
       {

	for (int i=0; i<60; i+=4)
	  {
           int j = (i+4);
	   Image[i+2] = (Image[i]+Image[j]) / 2;
	  }

	Image[62] = Image[60];
	image_quality += 1.0;	

	for (int i=0; i<62; i+=2)
	  {
           int j = (i+2);
	   Image[i+1] = (Image[i]+Image[j]) / 2;
	  }

	Image[63] = Image[62];
	gotcha = true;
       }
//     else  
//        With_Camera = false;

   return gotcha;
}




char t_robot_basic::__ImageRead_low(char *st)
{
	char gotcha = false;

   	int iq  = sscanf(st,"t,2,s,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
                      ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd",
      &Image[ 0],&Image[ 2],&Image[ 4],&Image[ 6],
      &Image[ 8],&Image[10],&Image[12],&Image[14],
      &Image[16],&Image[18],&Image[20],&Image[22],
      &Image[24],&Image[26],&Image[28],&Image[30],
      &Image[32],&Image[34],&Image[36],&Image[38],
      &Image[40],&Image[42],&Image[44],&Image[46],
      &Image[48],&Image[50],&Image[52],&Image[54],
      &Image[56],&Image[58],&Image[60],&Image[62]);
      
	image_quality = (double)iq / 32.0;

	if (iq == 32)
       {
	for (int i=0; i<62; i+=2)
	  {
	   Image[i+1] = (Image[i] & 0x0f) << 4;
	   Image[i] &= 0xf0;
	  }

	gotcha = true;
	image_quality += 1.0;	
       }
//     else  
//        With_Camera = false;

   return gotcha;
}

char t_robot_basic::__ImageRead_partial(char *st,int start, int len)
{
	if (start > (64 - len))
	   return false;

	int iq = 0;
	switch (len)
	  {
	   case 8:
   	     iq = sscanf(st,"t,2,r,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd",
	         &Image[start+ 0],&Image[start+ 1],&Image[start+ 2],&Image[start+ 3],
	         &Image[start+ 4],&Image[start+ 5],&Image[start+ 6],&Image[start+ 7]);

	     image_quality = (double)iq / 8.0;	
	     if (iq == 8)
	     	   return true;  	
	     break;

	   case 16:
   	     iq = sscanf(st,"t,2,r,%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd"
			        ",%hd,%hd,%hd,%hd,%hd,%hd,%hd,%hd",
	         &Image[start+ 0],&Image[start+ 1],&Image[start+ 2],&Image[start+ 3],
	         &Image[start+ 4],&Image[start+ 5],&Image[start+ 6],&Image[start+ 7],
	         &Image[start+ 8],&Image[start+ 9],&Image[start+10],&Image[start+11],
	         &Image[start+12],&Image[start+13],&Image[start+14],&Image[start+15]);

	     image_quality = (double)iq / 16.0;
	     if (iq == 16)
	     	return true;  	
	     break;	
	
	   default:
	     fprintf(stderr,"function not available\n");	
	     break;		
	  }

	return false;
}



char t_robot_basic::__GripperRead(char *st)
{
   int i;	
   if (sscanf(st,"t,1,h,%d",&i)==1)
      {
       if (i > 1)
	  {
	   arm_pos = i;
	   gr_update_flag = _m_GET_ARM_POSITION;
	  } 	
	else
	  {
	   gr_width = i;
	   gr_update_flag = _m_GET_GRIP_WIDTH; 	
	  } 
      }	
    else 
      return false;

   return true;
}


char t_robot_basic::__ReadGripOhmMeter(char *st)
{
    int i; 
    
    if (sscanf(st,"t,1,f,%d",&i) == 1)
	ohm = i;
     else 
       return false;


   return true;
}

char t_robot_basic::__CatchedObject(char *st)
{
    int i;

    if (sscanf(st,"t,1,g,%d",&i) == 1)
       {
        if (i == 255)
           catched = true;
          else
           catched = false;
       }	
	else 
	return false;

	return true;
}





















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