/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename :BicamGPS.cc                                                    */
/* Author   : K Headley                                                     */
/* Project  : Benthic Imaging AUV                                           */
/* Created  : 04/01/2008                                                    */
/****************************************************************************/
/* Modification History:                                                    */
/*  $Id: BicamGPS.cc,v 1.3 2009/02/11 01:55:46 headley Exp $   */
/*  $Name: FastSim $ */
/****************************************************************************/

#include "Bicam.h"
#include "BicamConsts.h"

/** Default destructor */
BicamGPS::~BicamGPS(){
  initialized=FALSE;
  initialize();
}
/** Initializing constructor 
    @param pXPort XPort to use
*/
BicamGPS::BicamGPS(XPort *pXPort){
  xport=pXPort;
  initialized=FALSE;
  initialize();
}
/** Initializing constructor 
    @param host XPort host name/address
    @param serialIPPort XPort TCP/IP port to use for serial I/O
*/
BicamGPS::BicamGPS(char *host, int serialIPPort){
  initialized=FALSE;
  connect(host,serialIPPort);
  initialize();
}
/** Default no argument constructor */
BicamGPS::BicamGPS(){
  initialized=FALSE;
  initialize();
}
/** Configure XPort parameters
    @param pXPort XPort to use
*/
int BicamGPS::configure(XPort *pXPort){
  xport=pXPort;
  return OK;
}

/** Establish XPort socket connections for serial communications.
    @param host XPort host name/address
    @param serialIPPort XPort TCP/IP port to use for serial I/O
 */
XPort *BicamGPS::connect( char *host,
			  int serialIPPort){
  XPortConnection *xpc;
  xpc=xport->getConnection();
  xpc->configHost(host);
  xpc->configSerial(serialIPPort);
  if(xport->connectxp(XPCON_SERIAL)==ERROR){
    cout << "connect: ERROR - connectxp failed"<<endl;
    return NULL;
  }
  
  return xport;
}
/** Initialize NMEA fields.
    BicamGPS sends GPGGA and GPRMC messages.
*/
int BicamGPS::initNMEA(){
  int errors=0;
  time_t now;
  struct timeval tvtime;
  time(&now);
  tvtime.tv_sec=now;
  tvtime.tv_usec=0;

  /* set GPGGA w/ min range values */  
  if(gpgga.setTIME(now)==ERROR){
    cout<<"ERROR: time (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setTIME(tvtime)==ERROR){
    cout<<"ERROR: tvtime (initNMEA)"<<endl;
    errors++;
  }

   if(gpgga.setLATD(-90)==ERROR){
    cout<<"ERROR: LATD (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setLATM(0.0000)==ERROR){
    cout<<"ERROR: LATM (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setLATH('N')==ERROR){
    cout<<"ERROR: LATH (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setLOND(-180)==ERROR){
    cout<<"ERROR: LOND (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setLONM(0.0000)==ERROR){
    cout<<"ERROR: LONM (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setLONH('W')==ERROR){
    cout<<"ERROR: LONH (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setQUAL(1)==ERROR){
    cout<<"ERROR: QUAL (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setSIU(3)==ERROR){
    cout<<"ERROR: SIU (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setHDOP(0.5)==ERROR){
    cout<<"ERROR: HDOP (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setALT(-9999.9)==ERROR){
    cout<<"ERROR: ALT (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setALTU('M')==ERROR){
    cout<<"ERROR: ALTU (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setGEOH(-99.9)==ERROR){
    cout<<"ERROR: GEOH (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setGEOHU('M')==ERROR){
    cout<<"ERROR: GEOHU (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setDUPT(0)==ERROR){
    cout<<"ERROR: DUPT (initNMEA)"<<endl;
    errors++;
  }
  if(gpgga.setDRID(0)==ERROR){
    cout<<"ERROR: DRID (initNMEA)"<<endl;
    errors++;
  }

  /* set GPRMC w/ min range values */  
  if(gprmc.setTIME(now)==ERROR){
    cout<<"ERROR: time (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setTIME(tvtime)==ERROR){
    cout<<"ERROR: tvtime (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setSTAT('A')==ERROR){
    cout<<"ERROR: STAT (initNMEA)"<<endl;
    errors++;
  }
 if(gprmc.setLATD(-89)==ERROR){
    cout<<"ERROR: LATD (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setLATM(54.3210)==ERROR){
    cout<<"ERROR: LATM (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setLATH('N')==ERROR){
    cout<<"ERROR: LATH (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setLOND(-179)==ERROR){
    cout<<"ERROR: LOND (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setLONM(12.3456)==ERROR){
    cout<<"ERROR: LONM (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setLONH('W')==ERROR){
    cout<<"ERROR: LONH (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setSOG(1.0)==ERROR){
    cout<<"ERROR: SOG (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setCOG(100.0)==ERROR){
    cout<<"ERROR: COG (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setDATE(now)==ERROR){
    cout<<"ERROR: DATE (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setMVM(10.0)==ERROR){
    cout<<"ERROR: MVM (initNMEA)"<<endl;
    errors++;
  }
  if(gprmc.setMVD('E')==ERROR){
    cout<<"ERROR: MVD (initNMEA)"<<endl;
    errors++;
  }
  
  if (errors>0){
    cout<<"initNMEA: ERROR there were "<<errors<<" errors"<<endl;
    return ERROR;
  }
    
  return OK;
}
/** @return degree portion of value specified in decimal degrees
    @param value decimal degrees
*/
int BicamGPS::degrees(double value){
  return (int)value;
}
/** @return decimal minutes portion of value specified in decimal degrees
    @param value decimal degrees
*/
double BicamGPS::decimalMinutes(double value){
  /* NOTE: sign conversion; always returns positive value */
  if(value<0)
    value=(-value);
  double dMin=60.0*(value-(int)value);
  return dMin;
}
/** @return GPGGA member variable */
GPGGA *BicamGPS::getGPGGA(){return &gpgga;}
/** @return GPRMC member variable */
GPRMC *BicamGPS::getGPRMC(){return &gprmc;}
/** @return XPort member variable */
XPort *BicamGPS::getXPort(){return xport;}

/** Set XPort member variable.
    @param xp Pointer to XPort
*/
void BicamGPS::setXPort(XPort *xp){
  xport=xp;
}
/** Initialize BicamGPS member variables */
int BicamGPS::initialize(){
  latitude=0.0;
  longitude=0.0;
  altitude=0.0;
  initNMEA();
  initialized=TRUE;
return OK;
}
/** Write NMEA GPGGA and GPRMC messages to XPort serial port.
    XPort should be configured and connected.
    NMEA messages should be initialized and/or updated.
*/
int BicamGPS::writeNMEA(){

  const char *pGPGGA,*pGPRMC;

   pGPGGA=gpgga.toString(NMEA::CRLF);
   //cout<<pGPGGA;
  if(pGPGGA!=NULL)
    xport->writeSerial(pGPGGA,strlen(pGPGGA),0);
  else
    perror("writeNMEA: GPGGA.toString");

  pGPRMC=gprmc.toString(NMEA::CRLF);
  //cout<<pGPRMC;
  if(pGPRMC!=NULL)
    xport->writeSerial(pGPRMC,strlen(pGPRMC),0);
   else
    perror("writeNMEA: GPRMC.toString");


  return OK;
}
/** Update lat, lon , alt, and time for NMEA messages.
    @param lat latitude in decimal degrees
    @param lon longitude in decimal degrees
    @param alt altitude in meters
*/
int BicamGPS::updatePosition( double lat, double lon, double alt){

  // this is not thread safe...
  latitude=lat;
  longitude=lon;
  altitude=alt;

  //cout<<setiosflags(ios::showpoint)<<setprecision(5)<<setw(8);
  //cout<<"lat:"<<lat<<" lon:"<<lon<<" alt:"<<alt<<endl;
  //cout<<"latd:"<<degrees(lat)<<" latm:"<<decimalMinutes(lat)<<endl;
  //cout<<"lond:"<<degrees(lon)<<" lonm:"<<decimalMinutes(lon)<<endl;

  // get lat, lon, alt here...
  gpgga.setLATD(degrees(lat));
  gpgga.setLATM(decimalMinutes(lat));
  gpgga.setLOND(degrees(lon));
  gpgga.setLONM(decimalMinutes(lon));
  gpgga.setALT(alt);

  gprmc.setLATD(degrees(lat));
  gprmc.setLATM(decimalMinutes(lat));
  gprmc.setLOND(degrees(lon));
  gprmc.setLONM(decimalMinutes(lon));

  return OK;
}
/** Update time for NMEA messages.
    @param timeVal timestamp to use for NMEA messages.
*/
int BicamGPS::updateTime( time_t timeVal){
  // update time
  gpgga.setTIME(timeVal);
  gprmc.setTIME(timeVal);
  gprmc.setDATE(timeVal);
  return OK;
}
/** Update time for NMEA messages.
    @param timeVal timestamp to use for NMEA messages.
*/
int BicamGPS::updateTime( struct timeval timeVal){
  // update time
  gpgga.setTIME(timeVal);
  gprmc.setTIME(timeVal);
  gprmc.setDATE(timeVal.tv_sec);
  return OK;
}

/** Update lat, lon , alt, and time (sec resolution) for NMEA messages.
    @param timeVal timestamp to use for NMEA messages.
    @param lat latitude in decimal degrees
    @param lon longitude in decimal degrees
    @param alt altitude in meters
*/
int BicamGPS::update(time_t timeVal, double lat, double lon, double alt){

  updatePosition(lat,lon,alt);
  updateTime(timeVal);
  return OK;
}
/** Update lat, lon , alt, and time (sub-sec resolution) for NMEA messages.
    @param timeVal timestamp to use for NMEA messages.
    @param lat latitude in decimal degrees
    @param lon longitude in decimal degrees
    @param alt altitude in meters
*/
int BicamGPS::update(struct timeval timeVal, double lat, double lon, double alt){
  updatePosition(lat,lon,alt);
  updateTime(timeVal);
  return OK;
}
/**@return latitude member variable */
double BicamGPS::getLat(){return latitude;}
/**@return longitude member variable */
double BicamGPS::getLon(){return longitude;}
/**@return altitude member variable */
double BicamGPS::getAlt(){return altitude;}