/* File: structDefs.cpp
 * -------------------
 * Written by: Debbie Meduna
 *****************************************************************************/

#include "structDefs.h"

/*----------------------------------------------------------------------------
/mapT member functions
/----------------------------------------------------------------------------*/
mapT::mapT()
{
   xpts = NULL;
   ypts = NULL;
   numX = 0;
   numY = 0;
}

mapT::~mapT()
{
   clean();
}

void mapT::clean()
{
   if(xpts != NULL)
   {
      delete [] xpts;
      xpts = NULL;
   }
  
   if(ypts != NULL)
   {
      delete [] ypts;
      ypts = NULL;
   }
  
   depths.CleanUp();
   depthVariance.CleanUp();
}

void mapT::reSampleMap(const double newRes)
{
   int newNumX, newNumY;
   double* xptsNew;
   double* yptsNew;
   Matrix depthsNew;
   Matrix depthVarNew;
   int i, j, subRes;
   Matrix subDepthMap;

   //Fill in new xpts/ypts vectors
   newNumX = int(round(fabs(xpts[numX-1]-xpts[0])/newRes))+1;
   newNumY = int(round(fabs(ypts[numY-1]-ypts[0])/newRes))+1;
   depthsNew.ReSize(newNumX, newNumY);
   depthVariance.ReSize(newNumX, newNumY);

   xptsNew = new double[newNumX];
   yptsNew = new double[newNumY];

   for(i = 0; i < newNumX; i++)
      xptsNew[i] = xpts[0]+newRes*i;
  
   for(j = 0; j < newNumY; j++)
      yptsNew[j] = ypts[0]+newRes*j;

   //Fill in new depth values
   if((newRes > dx) | (newRes > dy))
   {
      //TO DO: Fix this so it actually computes correct average for newRes!!
      subRes = int(ceil(newRes/dx));
      subDepthMap.ReSize(subRes, subRes);
      for(i = 1; i <= newNumX; i++)
      {
         for(j = 1; j <= newNumY; j++)
         {
            //fill in new depth values by averaging subMatrix
            subDepthMap = depths.SubMatrix((i-1)*subRes+1,i*subRes,
                                           (j-1)*subRes+1,j*subRes);
            depthsNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();

            //fill in new depth variance values
            subDepthMap = depthVariance.SubMatrix((i-1)*subRes+1,i*subRes,
                                                  (j-1)*subRes+1,j*subRes);
            depthVarNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();
         }
      }
   }
   else
      interp2mat(xpts,ypts,depths,xptsNew,yptsNew,depthsNew);

   //Remove old map and assign new values;
   clean();
   dx = newRes;
   dy = newRes;
   numX = newNumX;
   numY = newNumY;
   xpts = xptsNew;
   ypts = yptsNew;
   depths = depthsNew;
   depthVariance = depthVarNew;
   xcen = (xpts[numX-1] + xpts[0])/2.0;
   ycen = (ypts[numY-1] + ypts[0])/2.0;
}

//subSample the stored map to a lower resolution.
void mapT::subSampleMap(const int subRes)
{
   double newResX, newResY;
   int newNumX, newNumY, count,i, j;
   double* xptsNew;
   double* yptsNew;
   Matrix depthsNew;
   Matrix depthVarNew;
   Matrix subDepthMap(subRes, subRes);

   //Fill in new xpts/ypts vectors
   newNumX = int(numX/subRes);
   newNumY = int(numY/subRes);
   depthsNew.ReSize(newNumX, newNumY);
   depthVarNew.ReSize(newNumX, newNumY);

   xptsNew = new double[newNumX];
   yptsNew = new double[newNumY];

   count = 0;
   for(i = 0; i < numX && count < newNumX; i=i+subRes)
   {
      xptsNew[count] = xpts[i];
      count++;
   }
  
   count = 0;
   for(j = 0; j < numY && count < newNumY; j=j+subRes)
   {
      yptsNew[count] = ypts[j];
      count++;
   }

   //Fill in new depths matrix by averaging depths in the cells;
   for(i = 1; i <= newNumX; i++)
   {
      for(j = 1; j <= newNumY; j++)
      {
         //fill in new depth values by averaging subMatrix
         subDepthMap = depths.SubMatrix((i-1)*subRes+1,i*subRes,
                                        (j-1)*subRes+1,j*subRes);
         depthsNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();

         //fill in new depth variance values
         subDepthMap = depths.SubMatrix((i-1)*subRes+1,i*subRes,
                                        (j-1)*subRes+1,j*subRes) 
            - depthsNew(i,j);
         subDepthMap = SP(subDepthMap, subDepthMap);
         depthVarNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();
      }
   }

   newResX = dx*subRes;
   newResY = dy*subRes;

   //Remove old map and assign new values;
   clean();
   dx = newResX;
   dy = newResY;
   numX = newNumX;
   numY = newNumY;
   xpts = xptsNew;
   ypts = yptsNew;
   depths = depthsNew;
   depthVariance = depthVarNew;
   xcen = (xpts[numX-1] + xpts[0])/2.0;
   ycen = (ypts[numY-1] + ypts[0])/2.0;
}

//display map values in a more readable format
void mapT::displayMap()
{
   int i;

   //print a blank space in upper left corner
   output("%5s","");
   output("y:");

   //display ypt values
   for(i = 0; i < numY; i++)
      output("%5.2f", ypts[i]);
   output("\n x: \n");

   //display xpt values and depth values
   for(i = 0; i < numX; i++)
   {
      output("%5.2f", xpts[i]);
      output("%2s","");
      for(int j = 0; j < numY; j++)
         output("%5.2f", depths(i+1,j+1));
      
      output("\n");
   }
}

//copy assignment operator
mapT& mapT::operator=(mapT& rhs)
{
   if(this != &rhs)
   {
      this->clean();
      
      //copy non-array values
      dx = rhs.dx;
      dy = rhs.dy;
      xcen = rhs.xcen;
      ycen = rhs.ycen;
      numX = rhs.numX;
      numY = rhs.numY;
      depths = rhs.depths;
      depthVariance = rhs.depthVariance;
      
      //copy array values
      xpts = new double[numX];
      ypts = new double[numY];
      for(int i = 0; i < numX; i++)
         xpts[i] = rhs.xpts[i];
      for(int j = 0; j < numY; j++)
         ypts[j] = rhs.ypts[j];

   }
   return(*this);
}


/*----------------------------------------------------------------------------
/poseT member functions
/----------------------------------------------------------------------------*/
poseT::poseT()
{
   int i;

   //initialize values to zero
   x = 0.0;
   y = 0.0;
   z = 0.0;
   vx = 0.0;
   vy = 0.0;
   vz = 0.0;
   vw_x = 0;
   vw_y = 0;
   vw_z = 0;
   ax = 0.0;
   ay = 0.0;
   az = 0.0;
   phi = 0.0;
   theta = 0.0;
   psi = 0.0;
   wx = 0.0;
   wy = 0.0;
   wz = 0.0;
   time = 0.0;
   dvlValid = false;
   gpsValid = false;
   bottomLock = false;
   for(i = 0; i < 36; i++)
      covariance[i] = 0.0;
}


//copy assignment operator
poseT& poseT::operator=(poseT& rhs)
{
   if(this != &rhs)
   {
      //copy non-array values
      x = rhs.x;
      y = rhs.y;
      z = rhs.z; 
      vx = rhs.vx;
      vy = rhs.vy;
      vz = rhs.vz;
      vw_x = rhs.vw_x;
      vw_y = rhs.vw_y;
      vw_z = rhs.vw_z;
      ax = rhs.ax;
      ay = rhs.ay;
      az = rhs.az;
      phi = rhs.phi;
      theta = rhs.theta;
      psi = rhs.psi;
      wx = rhs.wx;
      wy = rhs.wy;
      wz = rhs.wz;
      time = rhs.time;
      dvlValid = rhs.dvlValid;
      gpsValid = rhs.gpsValid;
      bottomLock = rhs.bottomLock;
      //copy array values
      for (int i = 0; i < 36; i++)
         covariance[i] = rhs.covariance[i];
   }
   return(*this);
}

//difference assignment operator
poseT& poseT::operator-=(poseT& rhs)
{
   x -= rhs.x;
   y -= rhs.y;
   z -= rhs.z; 
   vx -= rhs.vx;
   vy -= rhs.vy;
   vz -= rhs.vz;
   vw_x -= rhs.vw_x;
   vw_y -= rhs.vw_y;
   vw_z -= rhs.vw_z;
   ax -= rhs.ax;
   ax -= rhs.ax;
   ay -= rhs.ay;
   az -= rhs.az;
   phi -= rhs.phi;
   theta -= rhs.theta;
   psi -= rhs.psi;
   wx -= rhs.wx;
   wy -= rhs.wy;
   wz -= rhs.wz;
   time -= rhs.time;
   dvlValid = (dvlValid && rhs.dvlValid);
   gpsValid = (gpsValid && rhs.gpsValid);
   bottomLock = (bottomLock && rhs.bottomLock);
   return(*this);
}

//addition assignment operator
poseT& poseT::operator+=(poseT& rhs)
{
   x += rhs.x;
   y += rhs.y;
   z += rhs.z; 
   vx += rhs.vx;
   vy += rhs.vy;
   vz += rhs.vz;
   vw_x += rhs.vw_x;
   vw_y += rhs.vw_y;
   vw_z += rhs.vw_z;
   ax += rhs.ax;
   ax += rhs.ax;
   ay += rhs.ay;
   az += rhs.az;
   phi += rhs.phi;
   theta += rhs.theta;
   psi += rhs.psi;
   wx += rhs.wx;
   wy += rhs.wy;
   wz += rhs.wz;
   time += rhs.time;
   dvlValid = (dvlValid && rhs.dvlValid);
   gpsValid = (gpsValid && rhs.gpsValid);
   bottomLock = (bottomLock && rhs.bottomLock);
   return(*this);
}

// Returns the number of bytes in serialized poseT when successful.
// Returns < 0 when there is insufficient space (difference between
// required and given)
//
int poseT::serialize(char *buf, int buflen)
{
  // Does the buffer have enough space?
  //
  int len = 55*sizeof(double) + 3*sizeof(char);
  if (len > buflen)
    return (buflen - len); // Space mismatch is returned to caller

  printf("Serializing poseT of size: %d\n",len);

  // Copy contents of m into buf
  //
  len = 0;
  memcpy(&buf[len], &x,    sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &y,    sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &z,    sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &vx,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &vy,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &vz,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &vw_x, sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &vw_y, sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &vw_z, sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &ax,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &ay,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &az,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &phi,  sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &theta,sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &psi,  sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &wx,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &wy,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &wz,   sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &time, sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &covariance, 36*sizeof(double)); len += 36*sizeof(double);

  // Use one byte for serialized booleans
  buf[len++] = dvlValid? 0x01 : 0x00;
  buf[len++] = gpsValid? 0x01 : 0x00;
  buf[len++] = bottomLock? 0x01 : 0x00;

  return len;
}

// Returns the number of bytes in serialized poseT when successful.
// Returns < 0 when there is insufficient space (difference between
// required and given)
//
int poseT::unserialize(char *buf, int buflen)
{
  // Does the buffer have enough space?
  //
  int len = 55*sizeof(double) + 3*sizeof(char);
  if (len > buflen)
    return (buflen - len);

  printf("UnSerializing poseT of size: %d\n", len);

  // Copy contents of m into buf
  //
  len = 0;
  memcpy(&x,    &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&y,    &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&z,    &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&vx,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&vy,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&vz,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&vw_x, &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&vw_y, &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&vw_z, &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&ax,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&ay,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&az,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&phi,  &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&theta,&buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&psi,  &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&wx,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&wy,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&wz,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&time, &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&covariance, &buf[len], 36*sizeof(double)); len += 36*sizeof(double);

  dvlValid = buf[len++] == 0x01;
  gpsValid = buf[len++] == 0x01;
  bottomLock = buf[len++] == 0x01;

  return len;
}

/*----------------------------------------------------------------------------
/measT member functions
/----------------------------------------------------------------------------*/
measT::measT()
{
   covariance = NULL;
   ranges = NULL;
   crossTrack = NULL;
   alongTrack = NULL;
   altitudes = NULL;
   measStatus = NULL;
   numMeas = 0; 
}

measT::~measT()
{
   clean();
}

//clean all dynamic memory elements of the struct
void measT::clean()
{
   if(covariance != NULL)
      delete [] covariance;
   covariance = NULL;
  
   if(ranges != NULL)
      delete [] ranges;
   ranges = NULL;
  
   if(crossTrack != NULL)
      delete [] crossTrack;
   crossTrack = NULL;
  
   if(alongTrack != NULL)
      delete [] alongTrack;
   alongTrack = NULL;
  
   if(altitudes != NULL)
      delete [] altitudes;
   altitudes = NULL;
  
   if(measStatus != NULL)
      delete [] measStatus;
   measStatus = NULL; 

   numMeas = 0;
}

//copy assignment operator
measT& measT::operator=(measT& rhs)
{
   int i;
   if(this != &rhs)
   {
      //if the two measT structs have different datatype or number of
      //measurements, we need to delete and recreate memory for the 
      //new measT struct.
      if(numMeas != rhs.numMeas || dataType != rhs.dataType)
      {
	//printf("Copying rhs to lhs, numMeas=%d\n", rhs.numMeas);
	this->clean();
	if(rhs.dataType == 2 || rhs.dataType == 4)
	{  
	  crossTrack = new double[rhs.numMeas];
	  alongTrack = new double[rhs.numMeas];
	  altitudes = new double[rhs.numMeas];
	}
	else
	  ranges = new double[rhs.numMeas];

	measStatus = new bool[rhs.numMeas];
	if(rhs.covariance != NULL)
	  covariance = new double[rhs.numMeas];           
      }
      
      //copy non-array values
      time = rhs.time;
      dataType = rhs.dataType;
      phi = rhs.phi;
      theta = rhs.theta;
      psi = rhs.psi;
      numMeas = rhs.numMeas;
      x = rhs.x;
      y = rhs.y;
      z = rhs.z;
      
      //copy array values
      for (i = 0; i < rhs.numMeas; i++)
      {
         if(rhs.dataType == 2 || rhs.dataType == 4)
         {
	    crossTrack[i] = rhs.crossTrack[i];
            alongTrack[i] = rhs.alongTrack[i];
            altitudes[i] = rhs.altitudes[i];
         }
         else
            ranges[i] = rhs.ranges[i];

         measStatus[i] = rhs.measStatus[i];
         if(rhs.covariance != NULL)
            covariance[i] = rhs.covariance[i];
      }

   }
   return(*this);
}

// Returns the number of bytes in serialized measT when successful.
// Returns < 0 when there is insufficient space (difference between
// required and given)
//
int measT::serialize(char *buf, int buflen)
{
  printf("Serializing measT...");
  // Does the buffer have enough space?
  //
  // Fixed length parts
  int nm = numMeas;
  int len = 7*sizeof(double) + 2*sizeof(int) + nm*sizeof(char);

  // Tracks, or just ranges?
  if (dataType == 2 || dataType == 4)
    len += (nm*3)*sizeof(double);
  else
    len += nm*sizeof(double);

  // Covariances?
  if (covariance)
    len += nm*sizeof(double);
  else
    dataType = 0 - dataType; // Signal lack of covariances with a dataType < 0

  printf("...with %d measurements, size: %d\n", nm, len);

  if (len > buflen)
    return (buflen - len);

  // Copy contents into buf. Order is significant!
  //
  len = 0;
  memcpy(&buf[len], &time,     sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &dataType, sizeof(int)   ); len += sizeof(int);
  memcpy(&buf[len], &phi,      sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &theta,    sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &psi,      sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &x,        sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &y,        sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &z,        sizeof(double)); len += sizeof(double);
  memcpy(&buf[len], &numMeas,  sizeof(int)   ); len += sizeof(int);

  // Copy the arrays
  //
  if (abs(dataType) == 2 || abs(dataType) == 4) {
    memcpy(&buf[len], crossTrack, nm*sizeof(double)); len += nm*sizeof(double);
    memcpy(&buf[len], alongTrack, nm*sizeof(double)); len += nm*sizeof(double);
    memcpy(&buf[len], altitudes,  nm*sizeof(double)); len += nm*sizeof(double);
  }
  else {
    memcpy(&buf[len], ranges,     nm*sizeof(double)); len += nm*sizeof(double);
  }

  if (covariance) {
    memcpy(&buf[len], covariance, nm*sizeof(double)); len += nm*sizeof(double);
    printf("measT has covariance values\n");
  }

  // Use one byte for serialized booleans
  for (int i = 0; i < nm; i++) buf[len++] = measStatus? 0x01 : 0x00;

  
  return len;
}

// Returns the number of bytes in serialized measT when successful.
// Returns < 0 when there is insufficient space (difference between
// required and given)
//
int measT::unserialize(char *buf, int buflen)
{
  // Copy contents of m into buf
  //
  int len = 0;
  clean();

  // Order is significant - must match serialize!
  //
  memcpy(&time,    &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&dataType,&buf[len], sizeof(int)   ); len += sizeof(int);
  memcpy(&phi,     &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&theta,   &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&psi,     &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&x,       &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&y,       &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&z,       &buf[len], sizeof(double)); len += sizeof(double);
  memcpy(&numMeas, &buf[len], sizeof(int)   ); len += sizeof(int);

  int nm = numMeas;
  printf("UnSerializing measT with %d measurements, fixed len:%d...", nm, len);

  if (nm > 0) {

    // Using ranges or tracks and altitudes?
    //
    if(abs(dataType) == 2 || abs(dataType) == 4) {
      crossTrack = new double[nm];
      alongTrack = new double[nm];
      altitudes = new double[nm];

      // Again, order is significant
      //
      memcpy(crossTrack, &buf[len], nm*sizeof(double)); len += nm*sizeof(double);
      memcpy(alongTrack, &buf[len], nm*sizeof(double)); len += nm*sizeof(double);
      memcpy(altitudes,  &buf[len], nm*sizeof(double)); len += nm*sizeof(double);
    }
    else {
      ranges = new double[nm];
      memcpy(ranges,     &buf[len], nm*sizeof(double)); len += nm*sizeof(double);
    }

    // A dataType of less than zero is a signal that there
    // are no covariances in this measT
    //
    if (dataType >= 0) {
      covariance = new double[nm];
      memcpy(covariance, &buf[len], nm*sizeof(double)); len += nm*sizeof(double);
      printf("measT has covariance values\n");
    }
    else
      dataType = 0 - dataType;

    // Finally, serialized booleans are single bytes
    //
    measStatus = new bool[nm];
    for (int i = 0; i < nm; i++) measStatus[i] = buf[len++] == 0x01;
  }

  return len;
}

/*----------------------------------------------------------------------------
/transformT member functions
/----------------------------------------------------------------------------*/
void transformT::displayTransformInfo()
{
   output("Rotation angles (phi, theta, psi): \n (%f ,%f, %f)\n", 
          rotation[0]*180/PI, rotation[1]*180/PI, rotation[2]*180/PI);
   output("Translation vector [dx, dy, dz]: \n (%f ,%f, %f)\n", dr[0], dr[1],
          dr[2]);
}

/*----------------------------------------------------------------------------
/sensorT member functions
/----------------------------------------------------------------------------*/
sensorT::sensorT()
{
   T_bs = NULL;
}

sensorT::sensorT(char* fileName)
{
   T_bs = NULL;
}

sensorT::~sensorT()
{
   if(T_bs != NULL)
      delete [] T_bs;
   T_bs = NULL;
}

void sensorT::parseSensorSpecs(char* fileName)
{
   fstream sensorFile;
   char temp[512];
   int i;
  
   sensorFile.open(fileName);
   if(sensorFile.is_open())
   {
      //read in sensor name
      sensorFile.ignore(256,':');
      sensorFile.getline(name,256);

      //read in sensor type
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      type = atoi(temp);

      //read in number of beams
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      numBeams = atoi(temp);

      //read in percent range error
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      percentRangeError = atof(temp);

      //read in beam width
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      beamWidth = atof(temp)*PI/180.0;

      //read in beam information
      T_bs = new transformT[numBeams];
        
      if(type == 2)
      {
         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         T_bs[0].rotation[1] = atof(temp)*PI/180.0;

         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         double dphi = atof(temp)*PI/180.0;

         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         T_bs[0].rotation[2] = atof(temp)*PI/180.0;

         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         double dpsi = atof(temp)*PI/180.0;

         for(i = 0; i < numBeams; i++)
         {
            T_bs[i].rotation[1] = T_bs[0].rotation[1] + i*dphi;
            T_bs[i].rotation[2] = T_bs[0].rotation[2] + i*dpsi;
            T_bs[i].rotation[0] = 0.0;
            T_bs[i].dr[0] = 0.0;
            T_bs[i].dr[1] = 0.0;
            T_bs[i].dr[2] = 0.0;
         }
      }
      else
      {
         //beam pitch angle
         sensorFile.ignore(256,':');
         for(i = 0; i < numBeams; i++)
         {     
            if(i < numBeams-1)
               sensorFile.getline(temp,10,',');
            else
               sensorFile.getline(temp,10);
            T_bs[i].rotation[1] = atof(temp)*PI/180.0;
            T_bs[i].rotation[0] = 0.0;
            T_bs[i].dr[0] = 0.0;
            T_bs[i].dr[1] = 0.0;
            T_bs[i].dr[2] = 0.0;
         }

         //beam yaw angle
         sensorFile.ignore(256,':');
         for(i = 0; i < numBeams; i++)
         {     
            if(i < numBeams-1)
               sensorFile.getline(temp,10,',');
            else
               sensorFile.getline(temp,10);
            T_bs[i].rotation[2] = atof(temp)*PI/180.0;
         }
      }

      sensorFile.close();
   }
   else
   {
      printf("Error opening file %s.  Exiting...\n", fileName);
      exit(0);
   }

   return;
}

void sensorT::displaySensorInfo()
{
   output("Sensor name: %s\n", name);
   output("Sensor type: %i\n", type);
   output("Number of beams per measurement: %i\n", numBeams);
}


/*----------------------------------------------------------------------------
/vehicleT member functions
/----------------------------------------------------------------------------*/
vehicleT::vehicleT()
{
   T_sv = NULL;
   sensors = NULL;
}

vehicleT::vehicleT(char* fileName)
{
   T_sv = NULL;
   sensors = NULL;
   parseVehicleSpecs(fileName);
}

vehicleT::~vehicleT()
{
   if(T_sv != NULL)
      delete [] T_sv;
   T_sv = NULL; 

   if(sensors != NULL)
      delete [] sensors;
   sensors = NULL;
}

void vehicleT::parseVehicleSpecs(char* fileName)
{
   fstream vehicleFile;
   char temp[512];
   char temp2[512];
   char sensorFile[1024];
   char* sensorPath;
   
   vehicleFile.open(fileName);
   if(vehicleFile.is_open())
   {
      //read in vehicleName
      vehicleFile.ignore(256,':');
      vehicleFile.getline(name,256);

      //read in number of sensors
      vehicleFile.ignore(256,':');
      vehicleFile.getline(temp,256);
      numSensors = atoi(temp);      

      //read in INS drift rate
      vehicleFile.ignore(256,':');
      vehicleFile.getline(temp,256);
      driftRate = atof(temp);

      //read in sensor information
      sensors = new sensorT[numSensors];
      T_sv = new transformT[numSensors];

      for(int i = 0; i < numSensors; i++)
      {
         //sensor name
         vehicleFile.ignore(256,':');
         vehicleFile.getline(sensors[i].name,256);

         //sensor orientation offset
         vehicleFile.ignore(256,':');
         vehicleFile.getline(temp,10,',');
         T_sv[i].rotation[0] = atof(temp)*PI/180.0;
         vehicleFile.getline(temp,10,',');
         T_sv[i].rotation[1] = atof(temp)*PI/180.0;
         vehicleFile.getline(temp,10);
         T_sv[i].rotation[2] = atof(temp)*PI/180.0;

         //sensor translational offset
         vehicleFile.ignore(256,':');
         vehicleFile.getline(temp,10,',');
         T_sv[i].dr[0] = atof(temp);
         vehicleFile.getline(temp,10,',');
         T_sv[i].dr[1] = atof(temp);
         vehicleFile.getline(temp,10);
         T_sv[i].dr[2] = atof(temp);
      
         //extract file directory
         strcpy(sensorFile, fileName);
         sensorPath = strstr(sensorFile,name);
         
         //determine sensor file name
         sprintf(temp2, "%s%s",sensors[i].name,"_specs.cfg\0");
         strcpy(sensorPath,temp2);

         //parse sensor file 
         sensors[i].parseSensorSpecs(sensorFile);
      }

      vehicleFile.close();
   }
   else
   {
      printf("Error opening file %s.  Exiting...\n", fileName);
      exit(0);
   }

   return;
}

void vehicleT::displayVehicleInfo()
{
   int i;

   output("Vehicle name: %s\n", name);
   output("Number of sensors: %i\n\n", numSensors);

   for(i = 0; i < numSensors; i++)
   {
      output("Sensor #%i: \n", i+1);
      sensors[i].displaySensorInfo();

      output("Sensor #%i to vehicle transformation information: \n", i+1);
      T_sv[i].displayTransformInfo();
      output("\n");
   }

}

commsT::commsT()
  : msg_type(0), parameter(0), vdr(0.0),
    mapname(NULL), cfgname(NULL)
{
}

commsT::commsT(char type)
  : msg_type(type), parameter(0), vdr(0.0),
    mapname(NULL), cfgname(NULL)
{
}

commsT::commsT(char type, char param)
  : msg_type(type), parameter(param), vdr(0.0),
    mapname(NULL), cfgname(NULL)
{
}

commsT::commsT(char type, char param, float dr)
  : msg_type(type), parameter(0), vdr(dr),
    mapname(NULL), cfgname(NULL)
{
}

commsT::commsT(char type, char param, char *map, char *cfg)
  : msg_type(type), parameter(param), vdr(0.0),
    mapname(NULL), cfgname(NULL)
{
  mapname = strdup(map);
  cfgname = strdup(cfg);
}

commsT::commsT(char type, char param, measT& m)
  : msg_type(type), parameter(param), vdr(0.0),
    mapname(NULL), cfgname(NULL)
{
  // Measure update message?
  //
  if ((msg_type == TRN_MOTN || msg_type == TRN_MEAS)) {
    mt = m;
  }
  printf("MU msg created\n");
}

commsT::commsT(char type, poseT& p)
  : msg_type(type), parameter(0), vdr(0.0),
    mapname(NULL), cfgname(NULL)
{
  // Measure update message?
  //
  if ((msg_type == TRN_MLE || msg_type == TRN_MMSE)) {
    pt = p;
  }
  printf("EP msg created\n");
}

commsT::~commsT()
{
  if (mapname) delete mapname;
  if (cfgname) delete cfgname;
}

int commsT::serialize(char *buf, int buf_length)
{
  printf("Serializing commsT\n");
  int len = 0;
  unsigned int ml;
  char *p_ml;
  memcpy(buf+len, &msg_type,  sizeof(msg_type));  len += sizeof(msg_type);
  memcpy(buf+len, &parameter, sizeof(parameter)); len += sizeof(parameter);
  p_ml = buf+len; len += sizeof(unsigned int); // reserve spot for msg length

  // Estimated position message?
  //
  if (msg_type == TRN_MLE || msg_type == TRN_MMSE) {
    len += pt.serialize(buf+len, buf_length - len);
  }
  // Measure update message?
  //
  else if (msg_type == TRN_MOTN || msg_type == TRN_MEAS) {
    len += mt.serialize(buf+len, buf_length - len);
  }
  // Vehicle drift rate?
  //
  else if (msg_type == TRN_SET_VDR) {
    memcpy(buf+len, &vdr, sizeof(vdr));
    len += sizeof(vdr);
  }
  // Initialization message?
  //
  else if (msg_type == TRN_INIT) {
    strcpy(buf+len, mapname); len += strlen(mapname)+1;
    strcpy(buf+len, cfgname); len += strlen(cfgname)+1;
  }

  ml = len - 2*sizeof(char) - sizeof(unsigned int);
  memcpy(p_ml, &ml, sizeof(ml));

  return len;
}

int commsT::unserialize(char *buf, int buf_length)
{
  printf("Unserializing commsT\n");
  int len = 0;
  unsigned int ml;

  memcpy(&msg_type,  buf+len, sizeof(msg_type));  len += sizeof(msg_type);
  //printf("msg_type:%c\n", msg_type);
  memcpy(&parameter, buf+len, sizeof(parameter)); len += sizeof(parameter);
  //printf("parameter:%d\n", parameter);
  memcpy(&ml, buf+len, sizeof(ml)); len += sizeof(ml);
  //printf("remaining:%d\n", ml);

  // Estimated position message?
  //
  if ((msg_type == TRN_MLE || msg_type == TRN_MMSE) && ml > 0) {
    //printf("Tell poseT to unserialize itself at buf[%d]\n", len);
    len += pt.unserialize(buf+len, buf_length - len);
  }
  // Measure update message?
  //
  else if ((msg_type == TRN_MOTN || msg_type == TRN_MEAS) && ml > 0) {
    //printf("Tell measT to unserialize itself at buf[%d]\n", len);
    len += mt.unserialize(buf+len, buf_length - len);
  }
  // Vehicle drift rate?
  //
  else if (msg_type == TRN_SET_VDR) {
    memcpy(&vdr, buf+len, sizeof(vdr));
    len += sizeof(vdr);
  }
  // Initialization message?
  //
  else if (msg_type == TRN_INIT) {
    mapname = strdup(buf+len); len += strlen(mapname)+1;
    cfgname = strdup(buf+len); len += strlen(cfgname)+1;
  }

  return len;
}

// Write a string representation of the object
//
char* commsT::to_s(char *buf, int buflen)
{
  if (buf) {
    if (buflen > 250) {
      if (msg_type != TRN_INIT) {
	mapname = NULL;
	cfgname = NULL;
      }
      int len = sprintf(buf, "commsT {type:%c|parameter:%d|vdr:%f|map:%s|cfg:%s|poseT time:%.2f|measT time:%.2f|numMeas:%d}",
			msg_type, parameter, vdr, mapname, cfgname, pt.time, mt.time, mt.numMeas);
      printf("%d\n", len);
    }
  }

  return buf;
}

// Clear state
//
void commsT::clean()
{
  msg_type = '*';
  parameter = 0;
  mt.clean();

  if (mapname) delete mapname;
  if (cfgname) delete cfgname;
  mapname = cfgname = NULL;

}