#include "vision_device.h"

extern Telemetry_String telstring;
extern char visionRxBuffer[];
extern int teleos_string_status;

VisionDevice::VisionDevice(char* devName, int freq, int type)
: SerialDevice(devName, freq, type)
{
	rxbuffer[0] = '\0';
        memset(visionRxBuffer, 0, 500);
}


VisionDevice::~VisionDevice()
{
	// release data???
}


bool
VisionDevice::parse()
{
      	static int teleos_string_timer;

        //teleos_string_status = 0;
        memset(this->rxbuffer, 0, 500);
        errno = 0;
	int n = read(this->handle(), this->rxbuffer, 500);
	
	if (n <= 0)
		{
		if(++teleos_string_timer > 200)  //Gives about 1/2 second update
			{
			tcflush(this->handle(), TCIFLUSH);
        		teleos_string_timer = 0;
        		switch (errno)
				{
				case EINTR:
					{
					sprintf(visionRxBuffer, "Error Code %d: Call Interrupted before data read", errno);
					break;
					}
				case EIO:
					{
					sprintf(visionRxBuffer, "Error Code %d: I/O Error (Low level)", errno);
					break;  	
					}
				case EAGAIN:	
					{
					sprintf(visionRxBuffer, "Error Code %d: No Data Available (Non Blocking)", errno);
					break;
					}
				case EISDIR:
					{
					sprintf(visionRxBuffer, "Error Code %d: Cannot opern a directory", errno);
					break;
					}
				case EBADF:
					{
					sprintf(visionRxBuffer, "Error Code %d: File not valid or not open", errno);
					break;
					}
				case EINVAL:
					{
					sprintf(visionRxBuffer, "Error Code %d: File unsuitable for raeding", errno);
					break;
			        }
				case EFAULT:
					{
					sprintf(visionRxBuffer, "Error Code %d: Buffer outside address space", errno);
					break;
					}
				default:
					{
					sprintf(visionRxBuffer, "Error Code %d: Unknown error type", errno);
					break;
				    }
				}
		    teleos_string_status = 0;	
			teleos_string_timer = 0;
			}
		return false;
        	}
 	this->rxbuffer[n] = '\0';
	memset(visionRxBuffer, 0, 500);
      	for(int i = 0; i<250; i++) visionRxBuffer[i] = rxbuffer[i];			
	visionRxBuffer[251] = '\0';
	teleos_string_status = 1;	
	return true;

}


void
VisionDevice::setSystemData()
{
	if (telstring.parse_and_set_string(this->rxbuffer) < 0)
		sprintf(visionRxBuffer, "Error: Parity or Framing Error");
}

int
VisionDevice::send()
{
	telstring.updateOutFields(2);		// for vision
	telstring.lineout(this->handle(), telstring.vision_concat());
		
	usleep(40000);   //gives 20 Hz Data rate   Dave
	
	return 0;
}
