#include "tms_device.h"

#include <sys/time.h>

extern Telemetry_String telstring; 
extern int tms_string_status;
int i_cab_out = 0;
int i_cab_speed = 0;
int i_tension = 0;
int i_pressure = 0;

TmsDevice::TmsDevice(char* devName, int freq, int type)
: SerialDevice(devName, freq, type) 
{}


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


bool
TmsDevice::parse()
{
	// local variables for the sscanf
	float tension,payout;
	int speed,chksum;

	int rtn;

	// local variables for managing the gui tms status 'led'
	time_t dummyTime;
	time_t currentTime;
	static time_t lastTime=0;
	int maxAge=5; // seconds before setting status 'bad'

	/********************************************************
	Tether Management System reader/parser for
	Ventana/RachelCarson 25-Oct-2012 r.schramm
	Replaces old Pt.Lobos tms parser.
	Expects NMEA-like message from the MacArtney winch
	such as:  RD,-0000072,000013.2,-02097.8,15132\n
	speed, tension, payout, chksum

	The serial port must be set to use canonical input,
    and opened with O_NDELAY.
    that is line buffering and non-blocking reads.
    (see commands.c for serial port settings)


	We will set the following globals:
	 	i_cab_speed
	 	i_tension
	 	i_cab_out
	 	i_pressure
	 	tms_string_status 

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

	currentTime = time(&dummyTime);

	// note: rxbuffer and its size delared in tms_device.h
	memset (this->rxbuffer, 0, TMSBUFSIZE); // first null fill the buffer

	rtn = read(this->handle(), this->rxbuffer, TMSBUFSIZE);


	if (rtn > 4) //want at least 3 characters so can test for RD,
	{

		this->rxbuffer[rtn] = 0; //force last char of string to 'null'

		//only parse if its an RD message
		if (this->rxbuffer[0] == 'R'
		 && this->rxbuffer[1] == 'D'
		 && this->rxbuffer[2] == ',')
		{ 
			rtn = sscanf(rxbuffer,"RD,%d,%f,%f,%d",
			&speed,&tension,&payout,&chksum);
			if(rtn > 3) { //sscanf should get 4 values
				// need to cast floats to integers to keep
				// compatable with old code elsewhere.
				i_cab_speed = speed;
				i_tension =  (int)(tension+0.5); //cheap rounding
				i_cab_out = (int)(payout+0.5);
				i_pressure = 0;
				tms_string_status = 1; //causes tms telem indicator to green
				lastTime = currentTime; //hold this time in static var
				return true;  //done
			}
		}
	}

	//falls thru to here if any of the tests above fail.
	if( (currentTime-lastTime) > maxAge) tms_string_status = 0;

	return false;

}


void
TmsDevice::setSystemData()
{
	//fprintf(stderr,"Tms: payout:%d speed:%d tension:%d\n",
	//			 i_cab_out,i_cab_speed,i_tension);
	telstring.TmsControlStringIn.cable_out = i_cab_out;
	telstring.TmsControlStringIn.cable_speed = i_cab_speed;
	telstring.TmsControlStringIn.tension = i_tension;
	telstring.TmsControlStringIn.pressure  = i_pressure;

}


int
TmsDevice::send()
{
	return 0;
}