/* File: structDefs.h
 * -------------------
 * structDefs defines various structs and their member functions for use in 
 * terrainNav type programs.
 *
 * Dependencies:
 * newmat*.h, myOutput.h, matrixArrayCalcs.h
 *
 * Written by: Debbie Meduna
 ******************************************************************************/

#ifndef _structDefs_h_
#define _structDefs_h_

#include "matrixArrayCalcs.h"
#include "myOutput.h"

#include "math.h"
#include <fstream>
#include "string.h"

#include <newmatap.h>
#include <newmatio.h>

#ifndef PI
#define PI 3.14159265358979
#endif

//!The mapT struct stores data in a gridded Matrix.
struct mapT {
  Matrix depths; //Positive down
  Matrix depthVariance;
  double* xpts; //North
  double* ypts; //East
  double dx, dy;
  double xcen, ycen;
  int numX, numY;

  mapT();
  ~mapT();
  void clean();
  void reSampleMap(const double newRes);
  void subSampleMap(const int subRes);
  void displayMap();
  mapT& operator=(mapT& rhs);
};

//!The poseT struct stores vehicle pose information.
struct poseT {
  double x, y, z; //North, East, Down
  double vx, vy, vz;
  double vw_x, vw_y, vw_z;
  double ax, ay, az;
  double phi, theta, psi;
  double wx, wy, wz;
  double time;
  bool dvlValid;
  bool gpsValid;
  bool bottomLock;
  double covariance[36];
  
  poseT();
  poseT& operator=(poseT& rhs);
  poseT& operator-=(poseT& rhs);
  poseT& operator+=(poseT& rhs);

  int   serialize(char *buf, int buflen);
  int unserialize(char *buf, int buflen);
};

//!The measT struct stores sonar measurement information.
struct measT {
  double time;
  int dataType; //1: DVL, 2: Multibeam, 3: Single Beam,
		//4: Homer Relative Measurement
  double phi, theta, psi;
  double x, y, z;
  double* covariance;
  double* ranges;
  double* crossTrack;
  double* alongTrack;
  double* altitudes;
  bool* measStatus;
  int numMeas;

  measT();
  ~measT();
  void clean();
  measT& operator=(measT& rhs);

  int   serialize(char *buf, int buflen);
  int unserialize(char *buf, int buflen);
};

//!The transformT struct stores a rotation and translation vector.
struct transformT
{
  double rotation[3];
  double dr[3];
  void displayTransformInfo();
};

//!The sensorT struct stores sonar sensor-specific information.
struct sensorT
{
  char name[256];
  int numBeams;
  double percentRangeError;
  double beamWidth;
  int type;  //should match type of associated measT measurements
  transformT* T_bs;

  sensorT();
  sensorT(char* fileName);
  ~sensorT();
  void parseSensorSpecs(char* fileName);
  void displaySensorInfo();
};

//!The vehicleT struct stores vehicle-specific information.
struct vehicleT
{
  char name[256];
  int numSensors;
  double driftRate;
  transformT* T_sv;
  sensorT* sensors;
  
  vehicleT();
  vehicleT(char* fileName);
  ~vehicleT();
  void parseVehicleSpecs(char* fileName);
  void displayVehicleInfo();
};

///////////////////////////////////////////////////////////////
// Definitions for client-server comms
//
#define TRN_MSG_SIZE 2048
#define TRN_PAYLOAD_SIZE (TRN_MSG_SIZE - 2*sizeof(char) - sizeof(unsigned int))

// Structure used to standardize marshalling client-server messages
// Usage:
//   {
//     char comms_buf[TRN_MSG_SIZE];
//     measT  mt;
//     :                                           // Do stuff with measT
//     commsT my_ct(TRN_MEAS, 0, 0, &mt);          // Create commsT
//     my_ct.serialize(comms_buf, TRN_MSG_SIZE);   // Flatten into buffer
//     send(sockfd, comms_buf, TRN_MSG_SIZE, 0);   // Send to server
//     recv(sockfd, comms_buf, TRN_MSG_SIZE, 0);   // Response
//     my_ct.unserialize(comms_buf, TRN_MSG_SIZE); // Look for ack/nack
//     if (my_ct.msg_type == TRN_ACK);             // OK
//     if (my_ct.msg_type == TRN_NACK);            // Problem
//   }
//
struct commsT
{
  char msg_type;
  char parameter;
  float vdr;
  poseT pt;
  measT mt;
  char *mapname;
  char *cfgname;

  commsT();
  commsT(char msg_type);
  commsT(char msg_type, char parameter);
  commsT(char msg_type, char parameter, float vdr);
  commsT(char msg_type, poseT& pt);
  commsT(char msg_type, char parameter, measT& mt);
  commsT(char msg_type, char parameter, char *map, char *cfg);
  ~commsT();

  char* to_s(char *buf, int buflen); // Write a string representation of the object
  void clean();                     // Clear state
  int   serialize(char *buf, int buflen=TRN_MSG_SIZE);
  int unserialize(char *buf, int buflen=TRN_MSG_SIZE);
};

// Conversations are initiated by the client through request messages.
// The server responds to all requests with either the information requested,
// or a ack/nack.
//
// TRN message are simple flat buffers of length TRN_MSG_SIZE.
// The first 6 bytes always consist of "header" information.
//   buf[0]:
//        char         msg_type;              // Message type (defined below)
//   buf[1]:
//        char         parameter;             // Simple parameter (1, 0, etc.)
//   buf[2*sizeof(char)]:
//        unsigned int msg_len                // Length of remaining message
//   buf[2*sizeof(char)+sizeof(unsigned int)]:
//        char         msg[TRN_PAYLOAD_SIZE]; // Payload (defined below)
//
/////////////////////////////////////////////////////////////////////////
// Message descriptions:
//

#define TRN_INIT 'I'

// Initialization messages.
// Client sends Map file name, vehicle config file name, and filter type.
// msg[0] = map file name, null-terminated string (variable length = M)
// msg[M] = vehicle config file name, null-terminated string (variable length = V)
// msg[M+V] = filter type, one byte.
//
// Example:
//  msg = "mapname.map\0confignamej.cfg\02" (2 is the number 0x02, not '2')
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MEAS 'M'

// Measure Update message from client to server.
// buf[1] = Sonar measurement type, one byte.
// msg[0] = serialized measT object (variable length = M).
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MOTN 'N'

// Motion Update messages from client to server drive the process.
// msg[0] = serialized poseT object (constant length = P).
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MLE 'E'

// MLE Estimated Position messages from client to server are requests for data from
// the server. The requests consist of only the message type (the rest of the
// message is ignored). The server reponds with a 'E' type message with a
// serialized poseT structure containing the requested data, or a nack message.
//
// From server:
// msg[0] = serialized poseT object representing MLE location (constance length = E)
//

#define TRN_MMSE 'S'

// MMSE Estimated Position messages from client to server are requests for data from
// the server. The requests consist of only the message type (the rest of the
// message is ignored). The server reponds with a 'S' type message with a
// serialized poseT structure containing the requested data, or a nack message.
//
// From server:
// msg[0] = serialized poseT object representing MMSE location (constance length = S)
//

#define TRN_SET_MW 'W'

// Set modified weighting message.
// buf[1] = 1 for true, 0 for false
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_FR 'F'

// Set filter reinit message.
// buf[0] = 1 for true, 0 for false
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_IMA 'A'

// Set Interpolate measurement attitude message.
// buf[0] = 1 for true, 0 for false
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_VDR 'D'

// Set drift rate message.
// msg[0] = float value
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_FILT_GRD 'G'

// Use filter grade message.
// buf[0] = 1 for high, 0 for low
// 
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_ACK '+'

// Ack last message. Request succeeded.
// Sent when no data other response is expected.
// 

#define TRN_NACK '-'

// Nack last message. Request failed.
// Sent when no data other response is expected, or a data request failed.
// 

#define TRN_BYE 'B'

// Optional message to close the link.
// No response expected.
// 

#endif