#include "gyro_device.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()
{
        static int ship_gyro_string_timer;

	int n;

	if ((n = read(this->handle(), this->gyroBuf, 36)) > 0) {
		if (n > 0) {
			if (helper(gyroBuf, n, &ship_heading)) {
				ship_gyro_string_status = 1;
				return true;
			}
		}
	}
	if(++ship_gyro_string_timer > 500000)
		{
		ship_gyro_string_status = 0;
		ship_gyro_string_timer = 0;
		}
	return false;
}



bool 
GyroDevice::helper(char* gyroBuf, int nBytes, float* gyro)
{
  char    *ptr;
  char    gyroStr[4];

  int mode, digitAddr;
  int digit;
  bool seqErr;
  char digitValue;

  *gyro = 0.0;

  for (ptr = gyroBuf + nBytes - 4; ptr > gyroBuf; ptr--)
  {
    digitValue = (*ptr & 0x0f) + '0';
    digitAddr = 3 - ((*ptr >> 4) & 0x03);
    mode = (*ptr >> 6) & 0x03;

    if ((digitAddr == 3) && (digitValue <= '9') && (mode == 0))
    {
      for (digit = 3, seqErr = false; digit >= 0; digit--)
      {
        digitValue = (*(ptr + digit) & 0x0f) + '0';
        digitAddr = 3 - ((*(ptr + digit) >> 4) & 0x03);
        mode = (*(ptr + digit) >> 6) & 0x03;

        if ((digitAddr != (3 - digit)) || (mode != 0))
          seqErr = true;
        else
          gyroStr[digitAddr] = digitValue;

      } /* for */

      if (!seqErr)
      {
        gyroStr[4] = 0;      /* Null terminate string that we built*/
                                /* Convert heading string to degrees  */

        *gyro = ((float) atoi(gyroStr) * 0.1);
        if ((*gyro > 360.0) || (*gyro == 0.0))
          for (digit = 3, seqErr = false; digit >= 0; digit--)
            {
              digitValue = (*(ptr + digit) & 0x0f) + '0';
              digitAddr = 3 - ((*(ptr + digit) >> 4) & 0x03);
              mode = ( *(ptr + digit) >> 6) & 0x03;
              //printf ("Digit Value: %c ", digitValue);
              //printf ("Digit Addr:  %d ", digitAddr);
              //printf ("Mode Ctrl:   %d ", mode);
              /*if (mode == 0x0)
                printf("Heading Data\n");

              else if (mode == 0x3)
                printf("Alarm Data\n");

              else
                printf("Mode Unknown\n");
             */  
	   }

	//printf("gyro: %f\n", *gyro);
        return(true);
      } /* if */
      //printf ("continuing\n");
    } /* if */

  } /* for */
  return (false);
}




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


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


