#include "gyro_device.h"

#include <sys/time.h>

extern Telemetry_String telstring; 
extern Vehicle vehicle;
extern int ship_gyro_string_status;
float	ship_heading = 0;

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


GyroDevice::~GyroDevice()
{
}

bool
GyroDevice::parse()
{
	float x; // local variable for the sscanf

	int rtn; //local var for return values from functions

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

	/**************************************************	
	GyroDevice reader/parser for Ventana/RachelCarson
	NMEA Ships Gyro string. 25-Oct-2012 r.schramm 
	Replaces old Pt.Lobos Sperry Lehmkuhl gyro
	raw binary parser.

	We set the following globals:
	ship_heading
	ship_gyro_string_status
	
	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 are looking for NMEA heading string like:
	$HEHDT,123.4,T\n
	
	We set the following globals :
	 ship_heading
	 ship_gyro_string_status

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

	currentTime = time(&dummyTime);

	// note: gyroBuf and its size delared in gyro_device.h
	memset(this->gyroBuf,0,GYROBUFSIZE); // null fill the buffer 

	rtn = read(this->handle(), this->gyroBuf, GYROBUFSIZE);
	
	if (rtn >= 14) { //need to have 14 chars of $HEHDT msg to test

   		this->gyroBuf[rtn] = 0; //force last char of string to 'null'
	   
		//parse if its an $HEHDT message
		if(this->gyroBuf[0] == '$'
		&& this->gyroBuf[1] == 'H' 
		&& this->gyroBuf[2] == 'E' 
		&& this->gyroBuf[3] == 'H' 
		&& this->gyroBuf[4] == 'D' 
		&& this->gyroBuf[5] == 'T'
		&& this->gyroBuf[13] == 'T' )
		{
			rtn = sscanf(gyroBuf, "$HEHDT,%f", &x);
			if (rtn > 0) { 
				ship_heading = x;
				ship_gyro_string_status = 1;
				lastTime = currentTime; //hold this time in static var
				return true;
			}
		}
	}

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

	return false;

}

void
GyroDevice::setSystemData()
{
	
	//cout << "gyro: "<<  ship_heading << endl;
	vehicle.ship_heading = ship_heading;
}


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