#include "GPS.h"
#include "Syslog.h"

#define BAUD 9600
#define PARITY "NONE"
#define STOP_BITS 1
#define DATA_BITS 8
#define READ_TIMEOUT 2000
#define GPS_MAX_REPLY 82
#define GPSPERIOD 1000

//GPS::GPS(void):Task("GPS")
//{
//  init();
//}

GPS::GPS(SerialDevice *device):PeriodicTask("GPS")
{
  _device = device;
  init();
  addPeriodicCallback(GPSPERIOD, (CallbackMethod)GPS::runGPS);
  _log = new GPSLog( this, DataLog::AsciiFormat );
}

GPS::~GPS()
{
  delete _output;
  delete _log;
}


int GPS::init()
{
  char buf[30];
  
  Syslog::write("GPS: initializing\n");
  _output = new GPSOutput();
  _outputData.updateTime.tv_sec = 0.0;
  _outputData.updateTime.tv_nsec = 0.0;

  try
    {
      _output->write(&_outputData);
    }
  catch (SharedData::AccessError error)
    {
      fprintf(stderr, "%s",error.msg);
      exit(1);
    }
  _device->commsDebugMode(SerialDevice::DebugOff);
  //_device->commsDebugMode(SerialDevice::DebugOn);
  
  // initialize serial port
  _device->setLineFormat(BAUD, DATA_BITS, STOP_BITS, PARITY);
  
  // clear serial port
  
  _device->clearPort();
  _device->flushReceiveBuf();
  
  // disable output sentences

  sprintf (buf, "$PGRMO,,2\xd\xa");
  _device->write(buf, strlen(buf));


  // enable output in GPGGA mode

  sprintf (buf, "$PGRMO,GPGGA,1\xd\xa");
  _device->write(buf, strlen(buf));
  
  Syslog::write("GPS: done initializing...\n");
  return OK;
  } 

int GPS::getData(void)
{
  // Syslog::write("GPS: getting data...\n");
  timespec utime;
  //  float range;
  int datagood;

  datagood = getGPS();
  
  //  cout << "datagood:" << datagood << endl;
  if (datagood!=-1)
    {
      //      Syslog::write("GPS: data is good\n");
      
      _outputData.UTCTime = UTCTime;
      _outputData.latitude = latitude;
      _outputData.NSHemisphere = NSHemisphere;
      _outputData.EWHemisphere = EWHemisphere;
      _outputData.longitude = longitude;
      _outputData.quality = quality;
      _outputData.numSatellites = numSatellites;
      _outputData.HDOP = HDOP;
      _outputData.antennaHeight = antennaHeight;
      _outputData.geoidalHeight = geoidalHeight;

      _outputData.updateTime.tv_sec = readtime.tv_sec;
      _outputData.updateTime.tv_nsec = readtime.tv_nsec;
      _log->write();

    }
  
  
  return OK;
}

int GPS::getGPS(void)
{
  /* loop through buffer until port is cleared to ensure most recent hit */

  char code, reply[GPS_MAX_REPLY];
  unsigned utc;
  int nconv, qual, sats, checksum;
  double lat, lon;
  float hdop, an_height, geo_height;
  char lat_ns, lon_ew;

  //  cout << "getting GPS\n";

  while(_device->nRecvdBytes() > 0)
    {
      if (_device->read(reply,GPS_MAX_REPLY,READ_TIMEOUT) == ERROR)
	return ERROR;
    }
  //  Syslog::write("GPS: reply: %s\n",reply);
  nconv = sscanf(reply, "$GPGGA,%u,%lf,%c,%lf,%c,%d,%d,",
		 &utc, &lat, &lat_ns, &lon, &lon_ew, &qual, &sats);

  if (nconv >  6)
    {
      UTCTime = utc;
      latitude = lat;
      NSHemisphere = lat_ns;
      EWHemisphere = lon_ew;
      longitude = lon;
      quality = qual;
      numSatellites = sats;

      // HDOP;
      //antennaHeight;
      //geoidalHeight;
      //DGPSdataAge;
      //DGPSReferenceStationID;
      
      clock_gettime(CLOCK_REALTIME,&readtime);
      // cout << range << endl;
    }
  else
    {
      //data is not good
      return -1;
    }

  return OK;
}


void GPS::runGPS()
{

  // read data
      
  getData();
  
  // and now write data

  try
    {
      _output->write(&_outputData);

    }
  catch (SharedData::AccessError error)
    {
      fprintf(stderr, "%s",error.msg);
      exit(1);
    }
  
}

int GPS::read(char *foo,int A, int B)
{
  return OK;
}

int GPS::write(char *foo, int A)
{
  return OK;
}