#ifndef _KEARFOT_H
#define _KEARFOT_H

#include "KearfottDefs.h"
#include "Serial.h"

class Kearfott {
public:


  enum KEARFOTT_MSG_ID {
    accept_configuration=2,
    accept_operational=4,
    return_configuration=6,
    navigation_data=1,
    configuration_data=3,
    slow_test_message=27,
    high_speed_IMU_data=19,
    doppler_PD4=21,
    gps_PVT_A=13,
    gps_PVT_B=15,
    gps_GGA=7,
    gps_GSA=9,
    gps_VTG=11
  };

  enum KEARFOTT_MSG_SUBID {
    IMU_raw_sums=1,
    IMU_compensated_sums=2,
    IMU_ctrl_loop1=3,
    IMU_ctrl_loop2=4,
    NAV_calibration=5,
    system_status=6,
    kalman_state_A=7,
    kalman_state_B=8,
    kalman_state_C=16,
    kalman_state_covar_A=9,
    kalman_state_covar_B=10,
    kalman_state_covar_C=18,
    observ_status=17,
    zupt_residual=11,
    gps_vel_residual=12,
    gps_pos_residual=13,
    dvl_vel_residual=14,
    pos_fix_residual=15
  };

  typedef struct {
    unsigned char cmdID;
    unsigned char numBytes;
    unsigned char data[255];
    unsigned char checksum;
  } KEARFOTT_MSG_TYPE;


  typedef struct {
    unsigned char validity;
    unsigned char modes;
    int init_lat;
    int init_lon;
    int idepth;
    short inithdg;
    unsigned short salinity;
    unsigned short cepos;
    unsigned char forcomm;
  } KEARFOTT_OP_TYPE;



  typedef struct {
    short valpha;
    short vbeta;
    short vgamma;
    unsigned char configBits;
    unsigned char satime;
    short vdelx;
    short vdely;
    short vdelz;
    short gpslx;
    short gpsly;
    short gpslz;
    short doplx;
    short doply;
    short doplz;
  } KEARFOTT_CONFIG_TYPE;
  

  //definitions for navigation data message (ID 1)
  enum KEARFOTT_NAV_MODE_TYPE {
    Idle = 0x00,
    Coarse_Stat_Align=0x01,
    Fine_Stat_Align_Incomplete=0x02,
    Fine_Stat_Align_Complete=0x03,
    Coarse_GPS_Align=0x04,
    Fine_GPS_Align_Incomplete=0x05,
    Fine_GPS_Align_Complete=0x06,
    Dop_Align_Incomplete=0x07,
    Dop_Align_Complete=0x08,
    Aided_Navigation=0x09,
    No_Go=0x0a
  };


  //data structure to hold raw state vector
  typedef struct {
    unsigned short cycles;
    unsigned char mode;
    unsigned char monitor;
    int lat;
    int lon;
    int depth;
    unsigned short bheight;
    short roll;
    short pitch;
    unsigned short heading;
    short vbodyx;
    short vbodyy;
    short vbodyz;
    short accelx;
    short accely;
    short accelz;
    short prate;
    short qrate;
    short rrate;
    unsigned int utcTime;
  } KEARFOTT_RAW_NAV_TYPE;

  //data structure to hold scaled state vector
  typedef struct {
    unsigned short cycles;
    unsigned char mode;
    unsigned char monitor;
    double lat;
    double lon;
    double northing;
    double easting;
    float depth;
    float bheight;
    float roll;
    float pitch;
    float heading;
    float vbodyx;
    float vbodyy;
    float vbodyz;
    float accelx;
    float accely;
    float accelz;
    float prate;
    float qrate;
    float rrate;
    float utcTime;
  } KEARFOTT_SCALED_NAV_TYPE;


  typedef struct {
    unsigned char dvlid; //data valid
    unsigned char datstr; //data structure
    unsigned short numbyte;
    unsigned char sysconfig;
    short xvelbtm;
    short yvelbtm;
    short zvelbtm;
    short evelbtm;
    unsigned short bm1;
    unsigned short bm2;
    unsigned short bm3;
    unsigned short bm4;
    unsigned short botstat;
    short xvelref;
    short yvelref;
    short zvelref;
    short evelref;
    short refstrt;
    short refend;
    unsigned char refstat;
    unsigned char toping_hr;
    unsigned char toping_min;
    unsigned char toping_sec;
    unsigned char toping_hun;
    unsigned short bit;
    unsigned short spdsnd;
    short temp;
    unsigned short checksum;
    unsigned short ktnav;
  } RDI_PD4_TYPE;
    
  /////////////////////////////////////////////////////////////////////////
  //// Sample time
  //virtual long sampleTime();
  //virtual short hours();
  //virtual short minutes();
  //virtual short seconds();
  //virtual short centiSeconds();

  /////////////////////////////////////////////////////////////////////////
  //// Latitude (degrees)
  //virtual double latitude();

  /////////////////////////////////////////////////////////////////////////
  //// Latitude hemisphere; 'N' or 'S'
  //virtual char nsHemisphere();

  /////////////////////////////////////////////////////////////////////////
  //// Longitude (degrees)
  //virtual double longitude();

  /////////////////////////////////////////////////////////////////////////
  //// Longitude hemisphere; 'E' or 'W'
  //virtual char ewHemisphere();

  /////////////////////////////////////////////////////////////////////////
  //// GPS Quality
  //virtual GpsIF::Quality quality();

  /////////////////////////////////////////////////////////////////////////
  //// Number of satellites
  //virtual short nSatellites();

  /////////////////////////////////////////////////////////////////////////
  //// Horizontal dilution of precision
  //virtual double hdop();

  /////////////////////////////////////////////////////////////////////////
  //// Altitude above geoid (mean sea level)
  //virtual double altitude();

  /////////////////////////////////////////////////////////////////////////
  //// Height of geoid above WGS84 ellipsoid
  //virtual double geoidHeight();

  /////////////////////////////////////////////////////////////////////////
  //// Age of DGPS data (seconds)
  //virtual short dgpsDataAge();

  /////////////////////////////////////////////////////////////////////////
  //// DGPS reference station ID
  //virtual short dgpsStationId();

  Kearfott(CSerial *device);
  ~Kearfott();
  BOOL _externalSensors;
  BOOL run();
  //void displayNavigationMessages(int skip);
  BOOL sendConfigData(KEARFOTT_CONFIG_TYPE *data);
  BOOL getConfigData(KEARFOTT_CONFIG_TYPE *data);
  BOOL sendOpData(KEARFOTT_OP_TYPE *data);
  BOOL parseMsg(KEARFOTT_MSG_TYPE *msg);
  void Kearfott::updateExternalSensors(
	BOOL usblValid, double usblLat, double usblLon,
	BOOL pressureValid, double pressureDepth,
	BOOL salinityValid, double salinity,
	BOOL gpsAid, BOOL dopAid, BOOL dopPing);

  KEARFOTT_SCALED_NAV_TYPE _scaledNavData;
  BOOL _newUSBLFix, _salValid, _depthValid, _ggaValid;
  double _usblLat, _usblLon, _currentDepth, _currentSal;
  char _gsaStr[255], _ggaStr[255], _vtgStr[255];

 private:

  //serial device
  CSerial *_device;

  //most recent data output. Need to add timestamps somewhere
  KEARFOTT_RAW_NAV_TYPE _rawNavData;

  KEARFOTT_CONFIG_TYPE _configData;
  KEARFOTT_OP_TYPE _opData;
  BOOL _opValid, _configValid;
  unsigned char _slowTestMsgData[64];

  unsigned char _highSpeedIMUData[28];
  RDI_PD4_TYPE _dopplerPD4;
  char _gpsGGA[512];
  char _gpsGSA[512];
  char _gpsVTG[512];

  //private methods
  BOOL initialize();

  BOOL parseConfigMessage(KEARFOTT_MSG_TYPE *msg);
  BOOL parseNavigationMessage(KEARFOTT_MSG_TYPE *msg);
  BOOL parseSlowTestMessage(KEARFOTT_MSG_TYPE *msg);
  BOOL parseHighSpeedIMUData(KEARFOTT_MSG_TYPE *msg);
  BOOL parseDopplerPD4(KEARFOTT_MSG_TYPE *msg);
  BOOL parseGPS_PVTA(KEARFOTT_MSG_TYPE *msg);
  BOOL parseGPS_PVTB(KEARFOTT_MSG_TYPE *msg);
  BOOL parseGPS_GGA(KEARFOTT_MSG_TYPE *msg);
  BOOL parseGPS_GSA(KEARFOTT_MSG_TYPE *msg);
  BOOL parseGPS_VTG(KEARFOTT_MSG_TYPE *msg);
  BOOL parseSlowTestMessage(unsigned char *data);
  BOOL parseFastTestMessage(unsigned char *data);
  BOOL sendMsg(KEARFOTT_MSG_TYPE *msg);
  BOOL readMsg(KEARFOTT_MSG_TYPE *msg);
  unsigned char checksum(KEARFOTT_MSG_TYPE *msg);
  void printRawNavType(KEARFOTT_RAW_NAV_TYPE *rawNav);
  void printScaledNavType(KEARFOTT_SCALED_NAV_TYPE *rawNav);
  void printDoppler(RDI_PD4_TYPE *pd4);
  void logMsg(KEARFOTT_MSG_TYPE *msg);

  HANDLE _rawLog;
};

#endif
