/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : GarminGps.cc                                                  */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "GarminGps.h"
#include "GpsUtils.h"
#include "Syslog.h"

#define BAUD 9600        // Sometimes 4800!!!
#define PARITY "NONE"
#define STOP_BITS 1
#define DATA_BITS 8
#define READ_TIMEOUT 2000
#define MaxRecordBytes 200
#define GPSPERIOD 1000


GarminGps::GarminGps(SerialDevice *device, SerialParameters *serialParams)
  : StreamSerialDriver("gpsDriver", device, MaxRecordBytes, "\xa",
		       READ_TIMEOUT)
{
  _device = device;
  _serialParams = serialParams;
  _log = new GpsLog( this, DataLog::BinaryFormat );
  _output = new GpsOutput(SharedData::Write);
}


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


DeviceIF::Status GarminGps::initialize()
{
  char buf[30];
  
  Syslog::write("Gps: initializing\n");
  _output->data.fix.sampleTime.seconds = 0.0;
  _output->data.fix.sampleTime.nanoSeconds = 0.0;

  _output->write();

  _device->commsDebugMode(SerialDevice::DebugOff);

  // initialize serial port
  _device->setLineFormat(_serialParams);
  
  // 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));
  
  _output->data.deviceReady = True;
  _output->write();
  Syslog::write("Gps: done initializing...\n");
  return DeviceIF::Ok;
} 


DeviceIF::Status GarminGps::processRecord(unsigned char *record, 
					  int nRecordBytes)
{
  Boolean debug = False;
  char *reply = (char *)record;

  dprintf("GarminGps::processRecord() - reply:\n%s", reply);

  try {
    GpsUtils::parseFix(reply, &_fix);
  }
  catch (Exception e) {
    // Data is not good
    Syslog::write("GarminGps::processRecord() - %s", e.msg);
    return DeviceIF::Error;
  }

  timespec sampleTime;

  // Fill in date and time (NMEA GGA standard apparently does NOT include
  // this!)
  clock_gettime(CLOCK_REALTIME, &sampleTime);
  _fix.sampleTime.seconds = sampleTime.tv_sec;
  _fix.sampleTime.nanoSeconds = sampleTime.tv_nsec;

  // Write to shared memory
  memcpy((void *)&_output->data.fix, (void *)&_fix, sizeof(GpsIF::Fix));
  _output->write();

  // Update log
  _log->write();

  return DeviceIF::Ok;
}


long GarminGps::sampleTime() 
{
  return _fix.sampleTime.seconds;
}


double GarminGps::latitude()
{
  return _fix.latitude;
}


char GarminGps::nsHemisphere()
{
  return GpsUtils::hemisphere(_fix.nsHemisphere);
}


double GarminGps::longitude()
{
  return _fix.longitude;
}


char GarminGps::ewHemisphere()
{
  return GpsUtils::hemisphere(_fix.ewHemisphere);
}

 
GpsIF::Quality GarminGps::quality()
{
  return _fix.quality;
}


short GarminGps::nSatellites()
{
  return _fix.nSatellites;
}


double GarminGps::hdop()
{
  return _fix.hdop;
}


double GarminGps::altitude()
{
  return _fix.altitude;
}


double GarminGps::geoidHeight()
{
  return _fix.geoidHeight;
}


short GarminGps::dgpsDataAge()
{
  return _fix.dgpsDataAge;
}


short GarminGps::dgpsStationId()
{
  return _fix.dgpsStationId;
}