/**************************************************************************
 * telemtry.cpp
 * ChangeLog:	telemtry constructor added w+ to fopen videoOverlay
 *			Mar. 3/ 2003
 *************************************************************************/
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/signal.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <string>
#include <pthread.h>
#include <signal.h>
#include <math.h>
#include "errors.h"
#include "telemetry.h"
#include "depth.h"
#include "alarm.h"
#include "io_element_control.h"
#include "parser.h"
#include "vehicle.h"
#include "control.h"
#include "io_poll.h"
#include "telemetry_string.h"
#include "graphic.h"





#ifdef __cplusplus
   extern "C" {
      #include "crc.c"

   }
#endif

#define MAX_GESOUT 	129
#define MAX_MPL 	65
#define BAUDRATE	B115200

extern int parse(char strToParse[], char delimeter, int tok_ns[], int nbuff, int ntok);

// This fucntion is defined in telemetry_string.cpp file
extern char* get_curr_time();

extern int send_status;
extern int receiving_data_counter;
extern int telemetry_status;
extern int globalAutoHeadOn;
extern int globalAutoDepthOn;
extern float auto_heading_setpoint;
extern float auto_depth_setpoint;
extern Vehicle vehicle;

extern int telemetry_receive_thread_timer;

//enum BOTCOMMANDS {AUTO_PILOT_INFO = 99};    //Dave Removed Aug 20 2004
enum BAUDCHOICE {baudrate9600, baudrate38400};
enum NSTOPBITS {oneStopBit = 1, twoStopBIts};
//enum ProtocolChoice {logSend, logRecv, visionSend, visionRecv};   //Dave removed

int		t_status;
pthread_t	dig_thread;
pthread_t	anal_thread;
pthread_t	recv_thread;
pthread_attr_t  t_detached_attr;
pthread_t thread;
pthread_t thread_2;
pthread_attr_t etached_attr;

int status;
int serial_fd;			// file descriptor for serial port communication
int fd_log;               	// file descriptor to write logging file
char endLine = '\n';		// needed for error logging file
char rx_buffer[1024];
char result_string[1024];
static char tx_buffer[512];
static int choice;			
static string command;

// ----------------- INITALIZE --------------------------------

int parse_error = 0 ;
int ground_channel = 0;
float ground_value = 0.0;
int ioElement_busy = 0;
int globalVar = 1;
int counter = 0;
char* devname = "/dev/ttyS0";
char* tx_buffer_ptr = &tx_buffer[0];
char* rx_buffer_ptr = &rx_buffer[0];
int armByte = 0;
int camByte = 0;
Telemetry telemetryObj;
void setup_serial(int fd);
float dvl_altitude = 0.0;
struct {
	long int bottom_x, bottom_y, bottom_z, bottom_e;
	long int raw_range[4];
	int bottom_status;
	long int water_x, water_y, water_z, water_e;
	int water_status, hour, min, sec, centisec;
	long int raw_temp, raw_pitch, raw_roll, raw_yaw;
	long int raw_dmgb_x, raw_dmgb_y, raw_dmgb_z, raw_dmgb_e;
	long int raw_dmgw_x, raw_dmgw_y, raw_dmgw_z, raw_dmgw_e;
	}
	dvl;


// ----------------------Telemetry data and ack bits---------------------

extern int cam_time_constant;
extern int arm_time_constant;
extern float dvl_heading;
int ack_gf_select, ack_dig_defaults, ack_ana_defaults, ack_teleos_enable;
int ack_latitude_setup, ack_rpm_setup, ack_arm_enable, ack_cam_enable, ack_auto_heading, ack_auto_depth;
int ack_auto_cruise, ack_auto_vision, ack_cam_samples_setup;
int ack_in_analog_gainoff, ack_arm_samples_setup, ack_auto_altitude;

int ack_autopilot, ack_arm, ack_cam;

/*
ack_dig_defaults, ack_ana_defaults,ack_use_aux_compass, ack_depth_jog;
*/
/************************************************************************/
Telemetry::Telemetry() throw()
{
	if ((serial_fd = open(devname, O_RDWR | O_NOCTTY)) < 0)
		{
		printf("Error, please try again\n");
		return;
		}
	setup_serial(serial_fd);

	// Initialize crc checking function	
	crcInit();

	// This variable return 1 if telemetry is running / established, otherwise return 0
	ioElement_busy = 0;

	// This open a file to log all the error in telemetry
	
	mode_t mode = S_IWUSR;	
	fd_log = open("../logging/telemetry_error.log", O_WRONLY | O_CREAT | O_TRUNC, mode);
        write(fd_log,\
        "The first two bytes are the type of transmission sent.\n\
        Following are the corresponding types that each code represents:\n\
        ACK_GESOUT = 0, ACK_OUT_MPL = 1, ACK_OUT_GESDAC = 2,\n\
        ACK_ALARM_POLL = 3, ANALOGS = 4, DEPTH = 5, OTHER_ALARM = 6, PITCHROLL = 7,\n\
        DIGITAL_ALARM = 8, HEADING = 9,	PILOT_FB = 10, ARM_FB= 11, CAM_FB = 12,\n\
        GROUND_FAULT = 13, ALL_ANALOGS = 14, ALL_DIGITALS = 15, RPM_SPEED = 16,\n\
        NORTH_VEL = 17, DVL_ALTITUDE = 18, DVL_HEADING = 19,\n\
	AUTO_CNTL_BYTE = 93, CAM_CNTL_BYTE = 94, ARM_CNTL_BYTE = 95,\n\
	TEL_TIMEDOUT = 100, TEL_PARSE_ERROR = 101, TEL_CRC_ERROR = 102,\n\
	TEL_FIFO_ERROR  = 103\n\n\n", 647);
}

Telemetry::~Telemetry()	throw()
{
	// ---------------- DESTRUCTION ---------------------------------
	// used to set the ventana-running state to OFF.
	//static char* param[] = { "-e", "/home/ventana/source/c++/ventana-running", "0", NULL };

	close(serial_fd);
	close(fd_log);
	//execvp("/usr/bin/kvt", param);	
}

/****************************************************************************/
void setup_serial(int fd)
{
	/**********************************************
         * baud_choice: 115200			      *
         * nStopBits: 	1 or 2			      *
         * parity:	0 for even, 1 for odd	      *
	 *********************************************/

	struct termios newtio;
	
	bzero(&newtio, sizeof(newtio));

	/* BAUDRATE: set bps rate
	 * CRTSCTS: not used
	 * CS8: 1 stop bit
         * CLOCAL: no modem control
         * CREAD: enable receiving characters
         */
	newtio.c_cflag = B115200 | CS7 | PARENB | CLOCAL | CREAD;

	/* IGNPAR: ignore bytes with parity errors	
	 * ICRNL: map CR to NL (otherwise a CR input on the other computer will not terminate input)
	 * otherwise make device raw (no other input processing)
         */
	newtio.c_iflag = INPCK | IMAXBEL | PARMRK | ICRNL;

	/* raw output */
	newtio.c_oflag &= ~OPOST;

        /* set input mode, noncanonical output  */
	newtio.c_lflag |= ICANON;	

	/* initialize all control characters, default in termio.h */
	newtio.c_cc[VINTR] 	= 0;
	newtio.c_cc[VQUIT] 	= 0;
	newtio.c_cc[VERASE] 	= 0;
	newtio.c_cc[VKILL] 	= 0;
	newtio.c_cc[VEOF] 	= 4;
	newtio.c_cc[VTIME] 	= 0;
	newtio.c_cc[VMIN] 	= 10;
	newtio.c_cc[VSWTC] 	= 0;
	newtio.c_cc[VSTART] 	= 0;
	newtio.c_cc[VSTOP] 	= 0;		/* no inter-character timer */
	newtio.c_cc[VSUSP] 	= 0;		/* no blocking */
	newtio.c_cc[VEOL] 	= 0;
	newtio.c_cc[VREPRINT] 	= 0;
	newtio.c_cc[VDISCARD] 	= 0;
	newtio.c_cc[VWERASE] 	= 0;
	newtio.c_cc[VLNEXT] 	= 0;
	newtio.c_cc[VEOL2] 	= 0;

	/* clean the line and activate settings for port */
	tcflush(fd, TCIOFLUSH);
	tcsetattr(fd, TCSANOW, &newtio);
}

/****************************************************************************/
int receive_packet(int fd, char* line)
{
	int c;
	int res;
	int nbytes = 0;
        char* tline = line;

	while((res = read(fd, &c, 1)) > 0)
		{
		if (isascii((char)c))
			{
			nbytes += res;
			*tline++ = (char)c;	
 			if ((char)c == 0x0a) return nbytes;
			}
		}
}

/****************************************************************************/
void lineout(int fd, char line[])
{
	// send through serial port
   	if (write(fd, line, strlen(line)) < 0) {
   		alarm_obj.set_other_alarm(23); //Generate Telemetry Alarm
	}
}

/*
//------------------------------------------------------------------
char* concat(char str[])
{
	//This string "str" should have contained the "control byte", and
	//the "body" separated with "|" delimeter
	//we will have to append the crc check "checksum" after them
		
	int checksum = 5;
	static char tx_ptr[512];
	sprintf(tx_ptr, "%s%c", str, '\n');
        return tx_ptr;
}
*/

//------------------------------------------------------------------
bool Telemetry::parse_and_set_values ()
{
   int elem_num;
   int other_alarm;
   int rpm_count;
   int index_tel[10];
   int index[100];
   int ntokens;
   int str_len;
   int i;
   int type;
 
   /* for crc */
   int crcInt;
   int crc_error_cnt = 0;

   int surfVehicle = 0;
   int digAnal = 0;
   float anal;
   float raw;
   float gain;
   float offset;
   float max;
   float min;
   char temp[512];		// local var to display error messages

   if (strcmp(rx_buffer, "PNG") == 0) return false;
   if (strcmp(rx_buffer, "REQ") == 0) return false;

   str_len = strlen(rx_buffer);
   ntokens = parse(rx_buffer, ' ', index_tel, str_len, 10);
   str_len = strlen(&rx_buffer[index_tel[0]]);

   if (ntokens == 2) 
   	{
	crcInt = crcCompute(&rx_buffer[index_tel[0]],  str_len);
	if (crcInt != atoi(&rx_buffer[index_tel[1]]))
		{
		tcflush(serial_fd, TCIFLUSH);
	
		//-------------- Write the bad packet to the file -------------
		snprintf(temp, 512, "bad crc from sub: %s\n%s\nCRC %s\n\n", get_curr_time(), &rx_buffer[index[0]], &rx_buffer[index_tel[1]]);
		int n = strlen(temp);
		write(fd_log, temp, n);
		bad_packet_counter++;
		crc_error_cnt++;  // do some sort of error checking
		return false;
		}
  	} // end if (ntokens == 2)
   else		// (ntokens != 2) 
   	{
	// wrong number of tokens
	tcflush(serial_fd, TCIFLUSH);
	
	//-------------- Write the bad packet to the file -------------	
		
	snprintf(temp, 512, "wrong # token from sub : %s\n%s\nCRC %s\n\n", get_curr_time(), &rx_buffer[index[0]], &rx_buffer[index_tel[1]] );
	int n = strlen(temp);
	write(fd_log, temp, n);
   	bad_packet_counter++;	
	return false;
   	}	// end else (ntokens != 2)

 
   // Now, parse the string after we get no crc error 
   //and the right number of tokens ( = 2 )
   
   str_len = strlen(rx_buffer);
   ntokens = parse(rx_buffer, '|', index, str_len, 100);

   if (ntokens > 0) type = atoi(&rx_buffer[index[0]]);
   else
   	{
      	tcflush(serial_fd, TCIFLUSH);
      	
      	//-------------- Write the bad packet to the file -------------	
	      		
	snprintf(temp, 512, "bad parsing message from sub: %s\n", get_curr_time());
	int n = strlen(temp);
	write(fd_log, temp, n);
       	bad_packet_counter++;
      	return false;
   	}		// end else (ntokens <= 0)
	
   float analogApplied;
   float analogRaw;
   int intRaw;
   int ind;
   int intRawPsi;

   switch(type)
   {
	case ALL_ANALOGS:
		if (ntokens != 66) break;
		for (ind = 1; ind <= 64; ind++) {
   			// if board is #1 or #3 --> unipolar
   			intRaw = atoi(&rx_buffer[index[ind]]);
  			if (ind >= 49 && ind <= 64) vehicle.update_vehicle_thruster(ind, intRaw);
 			if ((ind <= 16) || (ind > 32 && ind <= 48)) analogRaw = (float)intRaw*10/4095;
 			else    analogRaw = ((float)intRaw*10/4095) - 5.0;
 			analogApplied = ioElementControl.updateVehAnalog(ind, analogRaw);
		}
		break;	
// This one receives out single analog channel
	case ANALOGS:
   		if (ntokens != 3) break;
   		ind = atoi(&rx_buffer[index[1]]) + 1;
   		// if board is #1 or #3 --> unipolar
   		intRaw = atoi(&rx_buffer[index[2]]);
		//------------------------------------------------------------
		// This will be needed to get the values for vehicle
		// thrusters command
		 		
 		if (ind >= 49 && ind <= 64)vehicle.update_vehicle_thruster(ind, intRaw);
 		
 		if ((ind <= 16) || (ind > 32 && ind <= 48)) analogRaw = (float)intRaw*10/4095;
 		else analogRaw = ((float)intRaw*10/4095) - 5.0;
   		analogApplied = ioElementControl.updateVehAnalog(ind, analogRaw);
   		break;
  	case ACK_GESOUT:
  		ind = atoi(&rx_buffer[index[1]]);
  		// if the ind (element number / channel number ) is between
  		// 1 and 128, then it's a valid index for Gesout array
  		// Note: we dont have to add 1 to the index like the analog
  		// indexes above, becasue the sub has already sent up the
  		// offsetted index here
  		if (ind > 0 && ind < 129)
  			{
  		        status = atoi(&rx_buffer[index[2]]);
  		        vehicle.gesout_digitals[ind] = status;
  			}
  		break;
  	case ACK_OUT_MPL:
   		ind = atoi(&rx_buffer[index[1]]);
  		// if the ind (element number / channel number ) is between
  		// 1 and 65, then it's a valid index for MPL array
  		// Note: we dont have to add 1 to the index like the analog
  		// indexes above, becasue the sub has already send up the
  		// offsetted index here
  		if (ind > 0 && ind < 65)
  			{
  		        status = atoi(&rx_buffer[index[2]]);
  		        vehicle.mpl_digitals[ind] = status;
  			}
  		break;
	case CAM_FB:
		if (ntokens != 5) break;
		camByte = atoi(&rx_buffer[index[1]]);
    		vehicle.camera.feedback_shoulder = (float)(atoi(&rx_buffer[index[2]])) / 100 - 180 ;
    		vehicle.camera.feedback_pan = (float)(atoi(&rx_buffer[index[3]])) / 100 - 180;
    		vehicle.camera.feedback_tilt = (float)(atoi(&rx_buffer[index[4]])) / 100 - 180;
		break;
	case ARM_FB:
		if (ntokens != 7) break;
		armByte = atoi(&rx_buffer[index[1]]);
		vehicle.arm.feedback_shoulder = (float)(atoi(&rx_buffer[index[2]])) / 100;
		vehicle.arm.feedback_swing = (float)(atoi(&rx_buffer[index[3]])) / 100;
		vehicle.arm.feedback_elbow = (float)(atoi(&rx_buffer[index[4]])) / 100;
		vehicle.arm.feedback_pitch = (float)(atoi(&rx_buffer[index[5]])) / 100;
		vehicle.arm.feedback_yaw = (float)(atoi(&rx_buffer[index[6]])) / 100;
		break;
	case HEADING:	
		if (ntokens != 2) break;
		vehicle.curr_heading = (float)(atoi(&rx_buffer[index[1]])) /100;
		break;	
   	case DEPTH:
		if (ntokens != 3) break;
		intRaw = atoi(&rx_buffer[index[1]]);
   		analogRaw = (float) intRaw /100.0;
 		vehicle.curr_depth = analogRaw;
	        // now for the raw depth
		intRawPsi = atoi(&rx_buffer[index[2]]);
		vehicle.curr_raw_psi = (float) intRawPsi / 100.0;
		break;
	case PITCHROLL:
	        if (ntokens != 3) break;
		float pitch;
		float roll;
		pitch = (float) (atoi(&rx_buffer[index[1]])) / 100;
		roll = (float)(atoi(&rx_buffer[index[2]])) / 100;
	        vehicle.set_curr_pitch(pitch);
	        vehicle.set_curr_roll(roll);
		break;
	case DIGITAL_ALARM:
		//assert(&rx_buffer[index[1]]);
		elem_num = atoi(&rx_buffer[index[1]]);	
		alarm_obj.set_dig_alarm(elem_num);
		snprintf(temp, 512, "Digital Alarm Error : %s : %s \n", get_curr_time(), rx_buffer);
		write(fd_log, temp, strlen(temp));
		break;
	case OTHER_ALARM:
	        if (ntokens != 2) break;
		//assert(&rx_buffer[index[1]]);
		other_alarm = atoi(&rx_buffer[index[1]]);
		alarm_obj.set_other_alarm(other_alarm);
		snprintf(temp, 512, "Alarm Error : %s : %s \n", get_curr_time(), rx_buffer);
		write(fd_log, temp, strlen(temp));
		break;
    	case RPM_SPEED:
    	        if (ntokens != 2) break;
		//assert(&rx_buffer[index[1]]);
		vehicle.rpm_speed = 60 *(atoi(&rx_buffer[index[1]]));
		break;
	case NORTH_VEL:    // dvl data added Aug 2004 Dave
		//assert(&rx_buffer[index[1]]);
		if (ntokens != 33) break;
		vehicle.curr_vel = (float)(atoi(&rx_buffer[index[1]]));
		vehicle.curr_lat_vel = (float)(atoi(&rx_buffer[index[2]]));
		dvl.bottom_x = atol(&rx_buffer[index[3]]);
		dvl.bottom_y = atol(&rx_buffer[index[4]]);
		dvl.bottom_z = atol(&rx_buffer[index[5]]);
		dvl.bottom_e = atol(&rx_buffer[index[6]]);
		for(i=0; i<4; i++) dvl.raw_range[i]= atol(&rx_buffer[index[7+i]]);
		dvl.bottom_status = atoi(&rx_buffer[index[11]]);
		dvl.water_x = atol(&rx_buffer[index[12]]);
		dvl.water_y = atol(&rx_buffer[index[13]]);
		dvl.water_z = atol(&rx_buffer[index[14]]);
		dvl.water_e = atol(&rx_buffer[index[15]]);
		dvl.water_status = atoi(&rx_buffer[index[16]]);
		dvl.hour =     atoi(&rx_buffer[index[17]]);
		dvl.min =      atoi(&rx_buffer[index[18]]);
		dvl.sec =      atoi(&rx_buffer[index[19]]);
		dvl.centisec = atoi(&rx_buffer[index[20]]);
		dvl.raw_temp =   atol(&rx_buffer[index[21]]);
		dvl.raw_pitch =  atol(&rx_buffer[index[22]]);
		dvl.raw_roll =   atol(&rx_buffer[index[23]]);
		dvl.raw_yaw =    atol(&rx_buffer[index[24]]);
		dvl.raw_dmgb_x = atol(&rx_buffer[index[25]]);
		dvl.raw_dmgb_y = atol(&rx_buffer[index[26]]);
		dvl.raw_dmgb_z = atol(&rx_buffer[index[27]]);
		dvl.raw_dmgb_e = atol(&rx_buffer[index[28]]);
		dvl.raw_dmgw_x = atol(&rx_buffer[index[29]]);
		dvl.raw_dmgw_y = atol(&rx_buffer[index[30]]);
		dvl.raw_dmgw_z = atol(&rx_buffer[index[31]]);
		dvl.raw_dmgw_e = atol(&rx_buffer[index[32]]);
		break;
	case DVL_ALTITUDE:
		if (ntokens != 2) break;
		dvl_altitude = (float)(atoi(&rx_buffer[index[1]]))/100;
		vehicle.dvl_altitude = dvl_altitude;
		break;	
	case DVL_HEADING:
	        if (ntokens != 2) break;
		dvl_heading = (float)(atoi(&rx_buffer[index[1]]))/100;
		break;	
        case TEL_CRC_ERROR:
        	// fall through
        case TEL_PARSE_ERROR:
        	snprintf(temp, 512, "ERROR downstream serial: %s\n", get_curr_time());
		write(fd_log, temp, strlen(temp));
        	bad_packet_counter++;
        	break;
	case GROUND_FAULT:
	        if (ntokens != 3) break;
               	ground_channel = atoi(&rx_buffer[index[1]]);
	        ground_value = (float)(atoi(&rx_buffer[index[2]]) )*10/4095;
		break;
	case ACK_GF_SELECT:
               	ack_gf_select = 1;
	        break;
	case ACK_DIG_DEFAULTS:
               	ack_dig_defaults = 1;
	        break;
	case ACK_ANA_DEFAULTS:
               	ack_ana_defaults = 1;
	        break;
	case ACK_TELEOS_ENABLE:
               	ack_teleos_enable = 1;
	        break;
	case ACK_LATITUDE_SETUP:
               	ack_latitude_setup = 1;
	        break;
	case ACK_RPM_SETUP:
               	ack_rpm_setup = 1;
	        break;
	case ACK_ARM_ENABLE:
               	ack_arm_enable = 1;
               	break;
	case ACK_CAM_ENABLE:
               	ack_cam_enable = 1;
	        break;
	case ACK_AUTO_HEADING:
               	ack_auto_heading = 1;
	        break;
	case ACK_AUTO_DEPTH:
               	ack_auto_depth = 1;
	        break;
	case ACK_AUTO_ALTITUDE:
               	ack_auto_altitude = 1;
	        break;
	case ACK_AUTO_CRUISE:
               	ack_auto_cruise = 1;
	        break;
	case ACK_AUTO_VISION:
               	ack_auto_vision = 1;
	        break;
	case ACK_CAM_SAMPLES_SETUP:
               	ack_cam_samples_setup = 1;
	        break;	
	case ACK_IN_ANALOG_GAINOFF:
               	ack_in_analog_gainoff = 1;
	        break;
	case ACK_ARM_SAMPLES_SETUP:
               	ack_arm_samples_setup = 1;
	        break;
       	default:
		break;
   }
   return true;
}


/***************************************************************************************************************************************
  THREADING PROCEDURES

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

	int 	done_flag;		/* predicate for wait */
	pthread_cond_t	done = PTHREAD_COND_INITIALIZER;		/* wait for completion */

  	int 		running;
  	pthread_mutex_t	mutex = PTHREAD_MUTEX_INITIALIZER;
  	pthread_cond_t	request = PTHREAD_COND_INITIALIZER;


int tty_send_request (char cmd[])
{
	unsigned int crcInt;
	char* cmdPtr = cmd;
	static char buffer[1024];
	int n;	
        char tempDigital[1024];
	
        // for parsing
        int  index[10];
   	int 	ntokens;
   	int  str_len;
   	int i;
   	
   	cmdPtr = cmd;	
	while (*cmdPtr++ != '\n');
	*(--cmdPtr) = 0;
	n = strlen(cmd);
   	str_len = n;
   	
   	//---------------------------------------------------------------------
	
	crcInt = crcCompute(cmd, n);
	snprintf(buffer, 1024, "%s %u%c", cmd, crcInt, '\n');
	lineout(serial_fd, buffer); //send the whole command and crc to the serial port
	return 1;
}

//Called by commands.c Aug 20 2004
//For testing the defaults
/*
int tty_lineout(char* cmd)
{
	lineout(serial_fd, cmd);
        return 1;
}
*/

/***********************************************************************
 * start routine
 **********************************************************************/
int send_all_data_down()
{
	int n;
   	char pingBuffer[128];
   	
	receiving_data_counter = 3;
	
	// ---------- SENDING AUTOPILOT ------------------
					
	tty_send_request(vehicle.append_auto_string());
				
	do {
		memset(pingBuffer,0, 128);    	
	    	n = read(serial_fd, pingBuffer, 128);
		if (receiving_data_counter <= 0) return -1;
		if (n > 0) pingBuffer[n - 1] = 0;	
	} while (strcmp(pingBuffer, ACK_AUTO ) != 0);
	
	receiving_data_counter = 3;
			
	// ---------- SENDING ARM ------------------
						
	tty_send_request(vehicle.append_arm_string());
	
	do {
		memset(pingBuffer,0, 128);
	    	n = read(serial_fd, pingBuffer, 128);
		if (receiving_data_counter <= 0) return -1;
		if (n > 0) pingBuffer[n - 1] = 0;	
	} while (strcmp(pingBuffer, ACK_ARM ) != 0);
			
	receiving_data_counter = 3;
			
	//------------ SENDING THE GAIN-OFFSET DOWN --------------
	                                   	
	// This function will automatically send down all the
	// gain and offset for the sub analog channel
	ioElementControl.send_bottom_analog_down();
	ioElementControl.send_defaults_down();
	
 	//Send down the camera and arm time constants on power up
 	if((arm_time_constant > 0) && (arm_time_constant < 50))
	  	{	
	    	char temp[20];
	    	int loop_timeout = 0;
		ack_arm_samples_setup = 0;
		
	    	snprintf(temp, 10, "%d|%d\n", ARM_SAMPLE_SETUP, arm_time_constant);	
	 	tty_send_request(temp);
	 	usleep(400000);
	 	ack_arm_samples_setup = 1;
		while(ack_arm_samples_setup == 0)
			{
			tty_send_request(temp);
			usleep(400000);
			loop_timeout ++;
		    	if (loop_timeout == 10)
    				{
		    		ack_arm_samples_setup = 2;
		    		alarm_obj.set_other_alarm(38); //generate alarm
    				}
			}
	        }	
	if((cam_time_constant > 0) && (cam_time_constant < 50))	
	 	{	
		char temp[20];
		int loop_timeout = 0;
		ack_cam_samples_setup = 0;	
		
		snprintf(temp, 10, "%d|%d\n", CAM_SAMPLE_SETUP, cam_time_constant);	
	  	tty_send_request(temp);    							
	  	usleep(400000);
		ack_cam_samples_setup = 1;
		while(ack_cam_samples_setup == 0)
			{
			tty_send_request(temp);
			usleep(400000);
			loop_timeout ++;
		    	if (loop_timeout == 10)
    				{
		    		ack_cam_samples_setup = 2;
		    		alarm_obj.set_other_alarm(36); //generate alarm
    				}
			}
	   	}	
						
	// ---------- SENDING CAMERA ------------------
	tty_send_request(vehicle.append_cam_string());
	
	do {
		memset(pingBuffer,0, 128);
	    	n = read(serial_fd, pingBuffer, 128);
		if (receiving_data_counter <= 0)
			return -1;
	
		if (n > 0)
			pingBuffer[n - 1] = 0;	
			
				
	} while (strcmp(pingBuffer, ACK_CAM ) != 0);
	
	return 1;
}

/***********************************************************************
 * start routine
 **********************************************************************/
void start_routine()
{	
	int n;
   	char pingBuffer[128];
	telemetry_status = 0;
	long int timeout = 100;
	do
        {
        	write(serial_fd, "PNG\n", 8);
	        memset(pingBuffer,0, 128);
	    	n = read(serial_fd, pingBuffer, 128);
		if (n > 0) pingBuffer[n - 1] = 0; 	
        	
	} while (strcmp(pingBuffer, REQ) != 0);
	
        if (send_all_data_down() == 1)
	        {
         	telemetry_status = 1;
         	return;
        	}
}

/***********************************************************************
 * timeout routine
 *
 * sends an ack_png, gets the mpl and gesout status, sends an ACK_DIGITALS
 * checks the receiving_data_counter to make sure we haven't waited to long
 * the counter goes down in depth.cpp once a second
 * returns true on success
 **********************************************************************/
bool timeout_routine()
{

	int n;
   	char pingBuffer[128];
	char digitalBuffer[512];
	
        telemetry_status = 0;
	telemetryObj.bad_packet_counter = 0;	

	/************************************	
	 * ACK_AUTO = 93, AUTO_CNTL_BYTE = 94,
	 * ACK_CAM = 95, CAM_CNTL_BYTE = 96,
	 * ACK_ARM = 97, ARM_CNTL_BYTE = 98,
	 ************************************/
		int	index_tel[10];
   		int  	index[200];
   		int  	ntokens;
   		int  	str_len;
   		int 	i;
   		int 	type;
 
   		/* for crc */
   		int crcInt;
   		int crc_error_cnt = 0;

		// tell the sub we got the ping	
		write(serial_fd, ACK_PNG, 8);
								
		// we need all the sub's digitals into a tem char array digitalBuffer
		memset(digitalBuffer, 0, 512);
		n = read(serial_fd, digitalBuffer, 512);
		if (n > 0) digitalBuffer[n-1] = '\0';
	
   		str_len = strlen(digitalBuffer);
   		ntokens = parse(digitalBuffer, ' ', index_tel, str_len, 10);
   		str_len = strlen(&digitalBuffer[index_tel[0]]);
		if (ntokens == 2)
			{
			crcInt = crcCompute(&digitalBuffer[index_tel[0]],  str_len);
			if (crcInt != atoi(&digitalBuffer[index_tel[1]])) return false;
			}
		else  return false; // wrong number of tokens
		
   		str_len = strlen(digitalBuffer); 
   		ntokens = parse(digitalBuffer, '|', index, str_len, 200);
   
		/* extensive error checking */
		if (ntokens != 193) return false;
	        type = atoi(&digitalBuffer[index[0]]);
		if (type != ALL_DIGITALS) return false;
		for (i = 1; i <= 64; i++) vehicle.mpl_digitals[i] = atoi(&digitalBuffer[index[i]]);
		for (i = 65; i < 193; i++) vehicle.gesout_digitals[i - 64] = atoi(&digitalBuffer[index[i]]);		
		write(serial_fd, "ACK_DIGITALS\n", 13);

	do {
		memset(pingBuffer,0, 128);
	    	n = read(serial_fd, pingBuffer, 128);
		if (n > 0) pingBuffer[n - 1] = 0;
		if (receiving_data_counter <= 0) return false; //receiveing_data_counter decremented every second
	} while (strcmp(pingBuffer, REQ) != 0);
	
	if (send_all_data_down() == 1)	
		{
		telemetry_status = 1;
		return true;
		}		                        		
       return true;
}
	
/***********************************************************************
 * These are for RECEIVING routines
 *
 * These functions look almost exactly the same as the SENDING ones
 * above but since Send and Receive each has its own dedicated pin
 * and stream on serial port, therefore we have these two separate
 * structure that takes care the concurency for each issue
 * separately
 * Called by Commands.c
***********************************************************************/

void *tty_recv_routine (void *arg)
{
  	// using the select for serial ports 	
   	fd_set rset;
   	struct timeval tv;
   	int retval;
	int n;

	nice(-20);
	// clean the line and activate settings for port
	tcflush(serial_fd, TCIFLUSH);

	while(1) {
		memset(rx_buffer,0, 1024);	// clean up buffer
		n = read(serial_fd, rx_buffer, 1024);

		// reset the status to be large number
		receiving_data_counter = 2;
		
		if (n > 0)  rx_buffer[n - 1] = 0;
		if (strcmp(rx_buffer, PNG) == 0)    //get a ping?
			{
			int t_status;               //yes so start talking
			send_status = 0;
			t_status = timeout_routine();	// sends an ack_png, gets the mpl and gesout status, sends an ACK_DIGITALS
							// checks the receiving_data_counter to make sure we haven't waited to long
							// the counter goes down in depth.cpp once a second
							// returns true on success
			if (t_status)
				{
				telemetry_status = 1;
				char clear_alarm[10];
				
				// send down the command to clear the alarm list
				snprintf(clear_alarm, 10, "%d\n", CLEAR_ALARM_ARRAY);    	
      				tty_send_request(clear_alarm);
      	                        send_status = 1;
				}
			else
				{
			        sleep(3);               //this will let the receive_data_counter go to 0
				tcflush(serial_fd, TCIFLUSH);
				continue;
				}	
			}	
		else if (strcmp(rx_buffer, "ACK_AUTO") == 0)
			{
			ack_autopilot = 1; //not used becease we keep sending
			}
		else if (strcmp(rx_buffer, "ACK_ARM") == 0) 			
			{	
			ack_arm = 1; //not used because we keep sending
			}
		else if (strcmp(rx_buffer, "ACK_CAM") == 0)
			{	  			
  			ack_cam = 1; //not used beceause we keep sending
  			}
		else telemetryObj.parse_and_set_values();
		if(telemetry_receive_thread_timer < 100000) telemetry_receive_thread_timer++;		
		}			
	return NULL;
}