/* File: structDefs.cpp
 * -------------------
 * Written by: Debbie Meduna
 * 2014/??/??: Henthorn. Updated to support client/server architecture.
 * 2016/12/19: Henthorn. Updated to pass log dir name and particle file name to
 *             the server during initialization.
 *****************************************************************************/

#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);
	//printf("poseT time:%f\n", time);
	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);
	//printf("poseT time:%f\n", time);
	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;
	alphas = NULL;
	measStatus = NULL;
	numMeas = 0;
}

measT::~measT() {
	// although clean() exists
    // maybe preferable not to
    // call methods in destructor
    delete [] covariance;
    delete [] ranges;	
    delete [] crossTrack;	
    delete [] alongTrack;
    delete [] altitudes;
    delete [] alphas;
    delete [] measStatus;
}

// release all dynamic memory resources 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(alphas != NULL) {
		delete [] alphas;
	}
	alphas = NULL;
	
	if(measStatus != NULL) {
		delete [] measStatus;
	}
	measStatus = NULL;
	
	time = 0.;
	
	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) {
			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];
			}
			
   		alphas = 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];
			}

			alphas[i] = rhs.alphas[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");
	}

   // alphas
   memcpy(&buf[len], alphas, nm * sizeof(double));
	len += nm * sizeof(double);

	// printf("\t\t\tserialize - alphas[0] = %f, len = %d\n", alphas[0], len);
		
	// Use one byte for serialized booleans
	for(int i = 0; i < nm; i++) {
		buf[len++] = measStatus[i] ? 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) { //TODO buflen is unused?
	// 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;
		}
		if( dataType < 0 )
		  {
		    printf("measT::unserialize dataType = %d.\n", dataType);
		  }
		// Alphas
		//
   	alphas = new double[nm];
		memcpy(alphas, &buf[len], nm * sizeof(double));
		len += nm * sizeof(double);

		// printf("\t\t\tunserialize - alphas[0] = %f, len = %d\n", alphas[0], len);

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

/*----------------------------------------------------------------------------
/mbT member functions
/----------------------------------------------------------------------------*/
mbT::mbT() {
	time = 0.;
	numBeams = 0;
	ranges = NULL;
}

mbT::~mbT() {
	// although clean() exists
    // maybe best not to call methods
    // in destructor
	if(ranges != NULL) {
		delete [] ranges;
	}
	ranges = NULL;
	
	
	time = 0.;
	numBeams = 0;
}

// release all dynamic memory resources of the struct
void mbT::clean() {
	if(ranges != NULL) {
		delete [] ranges;
	}
	ranges = NULL;
	
	
	time = 0.;
	numBeams = 0;
}

//copy assignment operator
mbT& mbT::operator=(mbT& 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.
		this->clean();
		numBeams = rhs.numBeams;
		time = rhs.time;
		ranges = new float[numBeams];
		//copy range values
		for(i = 0; i < numBeams; i++) {
			ranges[i] = rhs.ranges[i];
		}
	}
	return(*this);
}

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

	//printf("...with %d beams, size: %d\n", numBeams, 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], &numBeams, sizeof(int));
	len += sizeof(int);
	// Copy the ranges
	//
	memcpy(&buf[len], ranges,     numBeams * sizeof(float));
	len += numBeams * sizeof(float);

	return len;
}

// Returns the number of bytes in serialized mbT when successful.
// Returns < 0 when there is insufficient space (difference between
// required and given)
//
int mbT::unserialize(char* buf, int buflen) { //TODO buflen is unused?
	// 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(&numBeams, &buf[len], sizeof(int));
	len += sizeof(int);
	int nm = numBeams;
	//printf("UnSerializing mbT with %d measurements, fixed len:%d...", nm, len);
	
	if(nm > 0) {
		ranges = new float[nm];
		memcpy(ranges,     &buf[len], nm * sizeof(float));
		len += nm * sizeof(float);
	}

	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", translation[0], translation[1],
		   translation[2]);
}

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

sensorT::sensorT(char* fileName) { //TODO filename unused?
	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;
	
	strcpy(filename, fileName);
	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);
		printf("parseSensorSpecs parsing sensor of type %d.\n",
		       type);
		
		//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].translation[0] = 0.0;
				T_bs[i].translation[1] = 0.0;
				T_bs[i].translation[2] = 0.0;
			}
		} 
		
		else if(type==5)
		{
			sensorFile.ignore(256,':');
			sensorFile.getline(temp,256);
			double init_phi = atof(temp)*PI/180.0;

			sensorFile.ignore(256,':');
			sensorFile.getline(temp,256);
			double dphi = atof(temp)*PI/180.0;
         
			// Centered on middle beam pointing down with beam 1 at the back
			for(i = 0; i < numBeams; i++)
			{
				T_bs[i].rotation[1] = init_phi - dphi*numBeams/2 + i*dphi;
				T_bs[i].rotation[2] = 0.0;
				T_bs[i].rotation[0] = 0.0;
				T_bs[i].translation[0] = 0.0;
				T_bs[i].translation[1] = 0.0;
				T_bs[i].translation[2] = 0.0;
			}
		}
		// DVL and others
			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].translation[0] = 0.0;
				T_bs[i].translation[1] = 0.0;
				T_bs[i].translation[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].translation[0] = atof(temp);
			vehicleFile.getline(temp, 10, ',');
			T_sv[i].translation[1] = atof(temp);
			vehicleFile.getline(temp, 10);
			T_sv[i].translation[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);
			printf("Sensor %d is of type %d.\n",
			       i, sensors[i].type);
		}
		
		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 member functions
/----------------------------------------------------------------------------*/
commsT::commsT()
	: msg_type(0), parameter(0), vdr(0.0),
	  mapname(NULL), cfgname(NULL), particlename(NULL), logname(NULL) {
}

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

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

commsT::commsT(char type, char param, float dr)// TODO char param unused
	: msg_type(type), parameter(0), vdr(dr),
	  mapname(NULL), cfgname(NULL), particlename(NULL), logname(NULL) {
}

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

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

commsT::commsT(char type, char param, mbT& m)
	: msg_type(type), parameter(param), vdr(0.0),
	  mapname(NULL), cfgname(NULL), particlename(NULL), logname(NULL) {
	//
	if((msg_type == TRN_MB)) {
		mb = m;
	} else {
		printf("MB msg NOT created\n");
	}
}

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

commsT::~commsT() {
<<<<<<< structDefs.cpp
	clean();
=======
	// release resources
    // allocated w/ strdup
    // though clean() exists, maybe
    // preferable not to call methods
    // in destructor
	if(mapname) {
		free(mapname);
	}
	if(cfgname) {
        free(cfgname);
	}
	if(particlename) {
		free(particlename);
	}
	if(logname) {
		free(logname);
	}
>>>>>>> 1.7.2.9
}

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_MOTN || msg_type == TRN_MLE || msg_type == TRN_MMSE) {
		len += pt.serialize(buf + len, buf_length - len);
	}
	// Measure update message?
	//
	else if(msg_type == TRN_MEAS) {
		len += mt.serialize(buf + len, buf_length - len);
	}
	// Multibeam update message?
	//
	else if(msg_type == TRN_MB) {
		len += mb.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;
		strcpy(buf + len, particlename);
		len += strlen(particlename) + 1;
		strcpy(buf + len, logname);
		len += strlen(logname) + 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_MOTN || 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_MEAS) && ml > 0) {
		//printf("Tell measT to unserialize itself at buf[%d]\n", len);
		len += mt.unserialize(buf + len, buf_length - len);
	}
	// Measure update message?
	//
	else if((msg_type == TRN_MB) && ml > 0) {
		//printf("Tell mbT to unserialize itself at buf[%d]\n", len);
		len += mb.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;
		particlename = strdup(buf + len);
		len += strlen(particlename) + 1;
		logname = strdup(buf + len);
		len += strlen(logname) + 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;
			}
			sprintf(buf, "commsT {type:%c|parameter:%d|vdr:%f|map:%s|cfg:%s|poseT time:%.2f|measT time:%.2f|numMeas:%d|mbT time:%.2f|numBeams:%d}",
					msg_type, parameter, vdr, mapname, cfgname, pt.time, mt.time, mt.numMeas, mb.time, mb.numBeams);
			/*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);*/
			//if (msg_type == TRN_MEAS) printf("alphas[0] = %f\n", mt.alphas[0]);
		}
	}
	
	return buf;
}

// Clear state
//
void commsT::clean() {
	msg_type = '*';
	parameter = 0;
	mt.clean();
<<<<<<< structDefs.cpp
	
	if(mapname) {
		free(mapname);
	}
	if(cfgname) {
		free(cfgname);
	}
	if(particlename) {
		free(particlename);
	}
	if(logname) {
		free(logname);
	}
	mapname = cfgname = particlename = logname = NULL;
=======
    // release resources
    // allocated w/ strdup
    if(mapname) {
        free(mapname);
    }
    if(cfgname) {
        free(cfgname);
    }
    if(particlename) {
        free(particlename);
    }
    if(logname) {
        free(logname);
    }
    mapname=NULL;
    cfgname=NULL;
    particlename=NULL;
    logname=NULL;
>>>>>>> 1.7.2.9
	
}