/* 
 * Thomas Pantzer: t_msgtype.cc, v0.1b  8.6.99 16:49 
 * 
 * 
 * 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 "utils.h"
#include "timer.h"

#include "khepera.h"
#include "t_msgtype.h"

#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <fcntl.h>
#include <ctype.h>

typedef struct {
		unsigned int id;
		int supressed,modcnt;
	        char msg_proto[64];  
	       } t_msg_charcodes;



#include "copyright.h"


void print_chr(char c)
{
	if (isprint(c))
	   fprintf(stderr,"%c",c);
	 else
	   fprintf(stderr,"[0x%02x]",c); 	   
}

/*
void t_khepera_msgs::reflect_values(char *next)
{

}
*/

long t_khepera_msgs::Parse(void)
{
	char *s,*next,*hold,*later;

	RobotContext *R = (RobotContext *)Rptr;

	next = &ReceiveBuf[0];
	hold = next;

	last_query_mask &= got_mask;
	last_query_mask |= query_mask;

	int i = 0;
	while (next != NULL)
	  {
#ifdef DEBUG
	   fprintf(stderr,"len:%d\tbuf(",strlen(ReceiveBuf));
	   for (unsigned int k=0; k<strlen(ReceiveBuf); k++)
	      print_chr(ReceiveBuf[k]);			
	   fprintf(stderr,")\n");	
#endif
	
	   s = strchr(next,'\n');
	   hold = next;

	   if (s != NULL)
	      {// we've got a complete msg
	       *s = '\0';		// cut the tail
	       i++;

	       later = s + 1;

#ifdef DEBUG
	       fprintf(stderr,"(% 3d.)",i);
	       for (unsigned int k=0; k<strlen(next); k++)
	         print_chr(next[k]);
	       fprintf(stderr,"later [%s]\n",later);
#endif
//	       reflect_values(next);	
	       switch(next[0])	    
		 {
		  case 't': // external bus
		    switch(next[2])
		      {
		       case '1': // gripper turret
			 switch(next[4])
			   {
			    case 'f':
			      if (R->__ReadGripOhmMeter(next) == true)
				 query_mask &= ~_m_GET_GRIP_OHM;
			      break;

			    case 'g':
			      if (R->__CatchedObject(next) == true)
				 query_mask &= ~_m_GET_OBJECT_DETECT;
			      break;

			    case 'h':
			      if (R->__GripperRead(next) == true)
				  query_mask &= ~R->gr_update_flag;
			      break;
			   }		
			 break;

		       case '2': // vision turret
			 switch(next[4])
			   {
			    case 'n':
			      R->__ImageRead(next);

			      query_mask &= ~_m_GET_CAMERA_IMAGE;
			      break;

			    case 's':
			      if (R->__ImageRead2(next) == true)
				 query_mask &= ~_m_GET_CAMERA_SUBSCAN2;
			      break;

			    case 'q':
			      if (R->__ImageRead_partial(next,partial8_start,8) == true)
				 query_mask &= ~_m_GET_CAMERA_PARTIAL8;
			      break;

			    case 'r':
			      if (R->__ImageRead_partial(next,partial16_start,16) == true)
				 query_mask &= ~_m_GET_CAMERA_PARTIAL16;
			      break;

			    case 't':
			      if (R->__ImageRead4(next) == true)
				 query_mask &= ~_m_GET_CAMERA_SUBSCAN4;
			      break;

			    case 'm':
			      if (R->__ImageRead_low(next) == true)
				 query_mask &= ~_m_GET_CAMERA_LOWRES;
			      break;

			    case 'l':
			      if (R->__GetIntensity(next) == true)
				 query_mask &= ~_m_GET_CAMERA_INTENSITY;
			      break;

			    case 'o':
			      if (R->__GetPixelMaxLight(next) == true)
				 query_mask &= ~_m_GET_CAMERA_MAXLIGHT;
			      break;

			    case 'p':
			      if (R->__GetPixelMinLight(next) == true)
				 query_mask &= ~_m_GET_CAMERA_MINLIGHT;
			      break;
			   }		
			 break;		
		      }
		    break; 

		   // basics	
		  case 'k':
		    if (R->__GetMotionCtrlFlags(next) == true)
		       query_mask &= ~_m_GET_STATUS;
		    break;

		  case 'e':
		    if (R->__GetMotorSpeed(next) == true)
		       query_mask &= ~_m_GET_MOTOR_SPEED;
		    break;

		  case 'h':	
		    if (R->__ReadWheelCounter(next) == true)
		       {
		        query_mask &= ~_m_GET_WHEEL_COUNTER;

			if ((query_mask & _m_GET_COORDS) > 0)
			   {
			    R->SolveCoords(true);
			    R->fresh_odo = 0;
//			    fprintf(stderr,"new coords available\n");
		            query_mask &= ~_m_GET_COORDS;
			   }
		       }		
		    break;

		  case 'o':
		    if (R->__ReadLEDs(1,next) == true)
		       query_mask &= ~_m_GET_AMBIENT_LED;
		    break;

		  case 'n':
		    if (R->__ReadLEDs(0,next) == true)
		       query_mask &= ~_m_GET_PROXIMITY_LED;
		    break; 

		  case 'c': query_mask &= ~_m_SET_WHEEL_POSITION;	break;
		  case 'g': query_mask &= ~_m_SET_WHEEL_COUNTER;	break;
		  case 'd': query_mask &= ~_m_SET_MOTOR_SPEED;		break;
		  case 'l': query_mask &= ~(_m_SET_LED0 | _m_SET_LED1);	break;
/*
	sprintf(st,"T,1,D,1\n");	query_mask |= _m_SET_GRIP_WIDTH;
	sprintf(st,"T,1,D,0\n");	query_mask |= _m_SET_GRIP_WIDTH;
	sprintf(st,"T,1,E,%d\n",n);	query_mask |= _m_SET_ARM_POSITION;

		  default:
		    fprintf(stderr,"unknown response");	
		    break;
*/
		 }

   	       next = later;
	      }
	    else
	       next = NULL;	
	   }	

	int j = hold - &ReceiveBuf[0];
#ifdef DEBUG
	fprintf(stderr,"j = %d, hold [%s]\n",j,hold);
#endif	
	got_mask = query_mask;
//	last_query_mask &= got_mask;

	return j;
}



void t_khepera_msgs::Init(char *r)
{
	Rptr = r;
	query_mask = 0;
	last_query_mask = 0;
	got_mask = 0;
	vision_timestamp = 0;

	RobotContext *R = (RobotContext *)Rptr;
	fcntl(R->_FD_, F_SETFL, O_NDELAY);
#ifdef DEBUG
	fprintf(stderr,"t_khepera_msgs::Init(R:0x%08lx,fd:%d)\n",(long)R,R->_FD_);
#endif
}

void t_khepera_msgs::Done(void)
{
}



int t_khepera_msgs::transfer_packet(void)
{
	t_msg_charcodes request_code[] = {
			 {_m_GET_PROXIMITY_LED	 ,0,0,"N\n"},
			 {_m_GET_AMBIENT_LED	 ,0,0,"O\n"},
			 {_m_GET_WHEEL_COUNTER	 ,0,0,"H\n"},
			 {_m_GET_MOTOR_SPEED	 ,0,0,"E\n"},
			 {_m_GET_STATUS		 ,0,0,"K\n"},
			 {_m_GET_GRIP_OHM	 ,0,0,"T,1,F\n"},
			 {_m_GET_OBJECT_DETECT	 ,0,0,"T,1,G\n"},
			 {_m_GET_GRIP_WIDTH	 ,0,0,"T,1,H,0\n"},
			 {_m_GET_ARM_POSITION	 ,0,0,"T,1,H,1\n"},
			 {_m_GET_CAMERA_IMAGE	 ,0,0,"T,2,N\n"},
			 {_m_GET_CAMERA_INTENSITY,0,0,"T,2,L\n"},
			 {_m_GET_CAMERA_MAXLIGHT ,0,0,"T,2,O\n"},
			 {_m_GET_CAMERA_MINLIGHT ,0,0,"T,2,P\n"},
			 {_m_GET_CAMERA_SUBSCAN2 ,0,0,"T,2,S\n"},
			 {_m_GET_CAMERA_SUBSCAN4 ,0,0,"T,2,T\n"},
			 {_m_GET_CAMERA_LOWRES   ,0,0,"T,2,M\n"},
			 {_m_GET_CAMERA_PARTIAL8 ,0,0,"T,2,Q\n"},
			 {_m_GET_CAMERA_PARTIAL16,0,0,"T,2,R\n"},
			 {0	 ,0,0,"\0"}
			};

	RobotContext *R = (RobotContext *)Rptr;
//	fprintf(stderr,"t_khepera_msgs::transfer_packet(0x%08lx)\n",(long)R);

	int i = 0;
	while (request_code[i].id != 0)
	  {
	   char st2[64];
	
//	   fprintf(stderr,"t_khepera_msgs::transfer_packet(%s) %d\n",request_code[i].msg_proto,i);
	   if ((request_code[i].id & query_mask) > 0)
	      {
	       request_code[i].modcnt++;
	
	       if ((_m_VISION_MASK & request_code[i].id) > 0)
		  {
		   long int t = GetTime();
//		   fprintf(stderr,"vts: %ld %ld\n",t,vision_timestamp);

		   if ((t - vision_timestamp) < 66)	 	
		      {
		       query_mask &= ~request_code[i].id;

     		       i++;
		       continue;
		      }	
		   
//		   fprintf(stderr,"vts-diff: %ld\n",t-vision_timestamp);
		   vision_timestamp = t; 
		  }

	       if ( ((request_code[i].id & last_query_mask) > 0) && 			
		    // das letzte mal schon requested
	            ((request_code[i].id & ~got_mask) == 0)  &&
		    // und noch nicht da
		    ((request_code[i].id & _m_MUCHDATA_MASK) > 0)  ) 
		    // und viel DATEN zu erwarten
		    { 
//		     last_query_mask &= ~request_code[i].id;
		     request_code[i].supressed++;				
		     query_mask &= ~request_code[i].id; 	
//		     if (request_code[i].supressed < 100)	
//			{
//		         fprintf(stderr,"p %d to %s",request_code[i].supressed+1,request_code[i].msg_proto);
		         i++;
		         continue;		
//			}
//		      else
//		        request_code[i].supressed = 0; 		
		    }		


	       switch (request_code[i].id)
	 	 {	
		  case _m_GET_CAMERA_PARTIAL8:
		    sprintf(st2,",%d",partial8_start);
		    strcat(DataPacket,request_code[i].msg_proto);
		    strcat(DataPacket,st2);		    
		    break;	

		  case _m_GET_CAMERA_PARTIAL16:
		    sprintf(st2,",%d",partial16_start);
		    strcat(DataPacket,request_code[i].msg_proto);
		    strcat(DataPacket,st2);		    
		    break;	

		  default:
		    strcat(DataPacket,request_code[i].msg_proto);
		    break;	
		 }
	      }
	    else
	      {
//	       if (request_code[i].supressed > 10)
//		  request_code[i].supressed--;	
	      }			
	   i++;
	  }		

//	sprintf(DataPacket,"N\n");

	i = strlen(DataPacket);
	int w;
	
	if (i > 0)
	   {
#ifdef DEBUG
	    fprintf(stderr,"transfer_packet() strlen:%d\t[%s]\t%d\n",i,DataPacket,R->fd);
#endif

	    w = write(R->_FD_,DataPacket,i);

	    if (w != i)
	       {
		fprintf(stderr,"didn't wrote complete message block\n");
	       }

	    query_mask &=  ~_m_SET_MASK;
	   }	
/* */
	DataPacket[0] = '\0';			// clear buffer

	int t = (i * 10000);
	t /= 38400;

	return t;
//	fprintf(stderr,"t_khepera_msgs::transfer_packet(end)\n");
} 


int t_khepera_msgs::receive_packet(void)
{
	RobotContext *R = (RobotContext *)Rptr;

#ifdef DEBUG
	fprintf(stderr,"t_khepera_msgs::receive_packet(R:0x%08lx,fd:%d)\n",(long)R,R->_FD_);
#endif

	if (FD_Ready(R->_FD_) == true)
	   {
	    int r = read(R->_FD_,&ReceiveBuf[readstart],MSG_QUEUE_SIZE);
	    if (r > 0)
	       {
                ReceiveBuf[readstart + r] = '\0';                                               
//              fprintf(stderr,"t_khepera_msgs::receive_packet(%s)\n",ReceiveBuf);              
                                                                                                
                long i = Parse();                                                               
                if (i > 0)                                                                      
                   { // uncomplete received message stays in buffer                             
                    strcpy(tmp_buf,&ReceiveBuf[i]);                                             
#ifdef DEBUG                                                                                    
                    fprintf(stderr,"copy [%s]\n",tmp_buf);                                      
#endif                                                                                          
                    strcpy(&ReceiveBuf[0],tmp_buf);                                             
                    readstart = strlen(ReceiveBuf);                                             
                                                                                                
//                  fprintf(stderr,"t_khepera_msgs::receive_packet(%s)\n",ReceiveBuf);          
                   }                                                                            
                 else                                                                           
                   readstart = 0;    

		got_mask = query_mask;                                                           
#ifdef DEBUG                                                                                    
                fprintf(stderr,"t_khepera_msgs::receive_packet(%d)\n",readstart);               
#endif                                                                                          
                int t = (r * 10000);                                                            
                t /= 38400;                                                                     
                                                                                                
                return t;                                                                       
	       }
	   }

	return 0;
}






void t_khepera_msgs::Delete_First(char C, char *st)
{
	char *s,*s2,*s3,/*s4,*/*s5;

//	fprintf(stderr,"delete something\n");

	s5 = st;

        do {
	    s = strchr(s5,C);
	    if (s == NULL)
	       return;

	    s3 = s - 1;

	    if (s3 > st)
	       { 
		if (*s3 != '\n')
		   { // weitersuchen
		    s5 = s + 1;
//		    s = strchr(s4,C);
	            continue;
		   } 
	       } 

	    break;
	   } while (true);


	s2 = strchr(s,'\n');
	if (s2 == NULL)
	   return;
	
	*s2 = '\0';
//	fprintf(stderr,"delete [%s]\t",s);

	s2++;
	strcpy(buf,s2);
	strcpy(s,buf);

//	fprintf(stderr,"new [%s]\t",st);
}


void t_khepera_msgs::send_wheelposition(long l, long r)
{
	char st[32];

	if (query_mask & _m_SET_WHEEL_POSITION)
	   Delete_First('C',DataPacket); 	  

	sprintf(st,"C,%ld,%ld\n",l,r);
	strcat(DataPacket,st);

	query_mask |= _m_SET_WHEEL_POSITION;
} 

void t_khepera_msgs::send_positioninfo(long l, long r)
{
	char st[32];

	if ((query_mask & _m_SET_WHEEL_COUNTER) > 0)
	   Delete_First('G',DataPacket); 	  

	sprintf(st,"G,%ld,%ld\n",l,r);
	strcat(DataPacket,st);

	query_mask |= _m_SET_WHEEL_COUNTER;
}

void t_khepera_msgs::send_motorspeed(int l, int r)
{
	char st[32];

//	fprintf(stderr,"1.[%s]\n",DataPacket);

	if ((query_mask & _m_SET_MOTOR_SPEED) > 0)
	   Delete_First('D',DataPacket); 	  

//	fprintf(stderr,"2.[%s]\n",DataPacket);

	sprintf(st,"D,%d,%d\n",l,r);
	strcat(DataPacket,st);

//	fprintf(stderr,"3.[%s]\n",DataPacket);

	query_mask |= _m_SET_MOTOR_SPEED;
}

void t_khepera_msgs::send_led(int num, int action)
{
	if (((query_mask & _m_SET_LED0) > 0) && (num == 0))
	   return;

	if (((query_mask & _m_SET_LED1) > 0) && (num == 1))
	   return;

	char st[32];
	sprintf(st,"L,%d,%d\n",num,action);
	strcat(DataPacket,st);

	if (num == 0)
	   query_mask |= _m_SET_LED0;
	 else
	   query_mask |= _m_SET_LED1;
}

void t_khepera_msgs::send_gripclose(void)
{
	if ((query_mask & _m_SET_GRIP_WIDTH) > 0)
	   return;

	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Gripper == false)
     	   return;	 

	char st[32];
	sprintf(st,"T,1,D,1\n");
	strcat(DataPacket,st);

	query_mask |= _m_SET_GRIP_WIDTH;
}

void t_khepera_msgs::send_gripopen(void)
{
	if ((query_mask & _m_SET_GRIP_WIDTH) > 0)
	   return;

	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Gripper == false)
     	   return;	 

	char st[32];
	sprintf(st,"T,1,D,0\n");
	strcat(DataPacket,st);

	query_mask |= _m_SET_GRIP_WIDTH;
}

void t_khepera_msgs::send_armpos(int n)
{
	if ((query_mask & _m_SET_ARM_POSITION) > 0)
	   return;

	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Gripper == false)
     	   return;	 

	char st[32];
	sprintf(st,"T,1,E,%d\n",n);
	strcat(DataPacket,st);

	query_mask |= _m_SET_ARM_POSITION;
}








//  are you ready ? --------------------------




void t_khepera_msgs::request_proximity(void)
{
	query_mask |= _m_GET_PROXIMITY_LED;
}

void t_khepera_msgs::request_ambient(void)
{
	query_mask |= _m_GET_AMBIENT_LED;
}

void t_khepera_msgs::request_wheelposition(void)
{
	query_mask |= _m_GET_WHEEL_COUNTER;
} 

void t_khepera_msgs::request_coords(void)
{
	query_mask |= _m_GET_COORDS;
	query_mask |= _m_GET_WHEEL_COUNTER;
} 

void t_khepera_msgs::request_motorspeed(void)
{
	query_mask |= _m_GET_MOTOR_SPEED;
}

void t_khepera_msgs::request_status(void)
{
	query_mask |= _m_GET_STATUS;
}

void t_khepera_msgs::request_gripwidth(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Gripper == false)
     	   return;	 

	query_mask |= _m_GET_GRIP_WIDTH;
}


void t_khepera_msgs::request_armpos(void)
{
	query_mask |= _m_GET_ARM_POSITION;
}


void t_khepera_msgs::request_resistor(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Gripper == false)
     	   return;	 

	query_mask |= _m_GET_GRIP_OHM;
}

void t_khepera_msgs::request_contact(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Gripper == false)
     	   return;	 

	query_mask |= _m_GET_OBJECT_DETECT;
}


void t_khepera_msgs::request_brightness(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Camera == false)
     	   return;	 

	query_mask |= _m_GET_CAMERA_INTENSITY;
}

void t_khepera_msgs::request_maxlight(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Camera == false)
     	   return;	 

	query_mask |= _m_GET_CAMERA_MAXLIGHT;
}

void t_khepera_msgs::request_minlight(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Camera == false)
     	   return;	 

	query_mask |= _m_GET_CAMERA_MINLIGHT;
}

void t_khepera_msgs::request_image(void)
{
	RobotContext *R = (RobotContext *)Rptr;

  	if (R->With_Camera == false)
     	   return;	 

	query_mask |= _m_GET_CAMERA_IMAGE;
}
	
void t_khepera_msgs::request_subscan2(void)
{
	query_mask |= _m_GET_CAMERA_SUBSCAN2;
}


void t_khepera_msgs::request_subscan4(void)
{
	query_mask |= _m_GET_CAMERA_SUBSCAN4;
}


void t_khepera_msgs::request_lowres_scan(void)
{
	query_mask |= _m_GET_CAMERA_LOWRES;
}


void t_khepera_msgs::request_partial_scan8(int start)
{
	query_mask |= _m_GET_CAMERA_PARTIAL8;
	partial8_start = start;
}

void t_khepera_msgs::request_partial_scan16(int start)
{
	query_mask |= _m_GET_CAMERA_PARTIAL16;
	partial16_start = start;
}







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







