//////////////////////////////////////////////////////////////////////
// traject.cpp: some source for playing with trajectory structures
//              mainly in C, not C++
// Author: Brian Leibowitz
//////////////////////////////////////////////////////////////////////

#include <stdlib.h>
#include <iostream.h>
#include <fstream.h>
#include <except.h>

#include "../include/traject/traject.h"
#include "../include/spfa/SPFAmod.h"


Trajectory& Trajectory::scale(float scalefactor)
{
    int i, j;

	for (i = 0; i < numcoords; i++) {
        for (j = 1; j <= NUM_AXES; j++) {
			coord[i][j] *= scalefactor;
        }
	}

	return *this;
}

Trajectory::Trajectory()
:	numcoords(0), coord(NULL)
{}

Trajectory::Trajectory(int numOfCoords)
:	numcoords(numOfCoords)
{
	coord = new coordinate[numcoords];
}


Trajectory::Trajectory(char* filename)
{
	readFile(filename);
}

Trajectory::~Trajectory()
{
	if ( coord != NULL )
		delete coord;

}

#define MAX_POINTS 1000
Trajectory& Trajectory::readFile(char* filename)
{
	int i;
	ifstream inFile;
	
	inFile.open( filename, ios::in );
	if( !inFile )   {
		return *(Trajectory*)NULL;
	}

	try {
		coord = new coordinate[MAX_POINTS];
	} catch (xalloc) {
		return *(Trajectory*)NULL;
	}

	for( i = 0; i<MAX_POINTS && !inFile.eof(); ++i ) {
				inFile  >>      coord[i][0]   		// time
						>>      coord[i][X+1]
						>>      coord[i][Y+1]
						>>      coord[i][Z+1]
						>>      coord[i][ROLL+1]
						>>      coord[i][PITCH+1]
						>>      coord[i][YAW+1];
	}

	if ( inFile.eof() ) {   // if the whole file is read
		numcoords = i-1;      // the last line is read as  0 0 ... 0, not very elegant
	}
	else {
		numcoords = i;
//		cerr << "Input data truncated to " << i << " points" << endl;
	}
	coord = (coordinate*)realloc(coord, numcoords*sizeof(coordinate));

	return *this;
}


int Trajectory::writeFile(char* filename)
{
	int i;
	ofstream outFile;
	
	outFile.open( filename, ios::out );
	if( !outFile )   {
		return false;
	}

	for( i = 0; i<numcoords; ++i ) {
				outFile <<      coord[i][0]			<< '\t'// time
						<<      coord[i][X+1]		<< '\t'
						<<      coord[i][Y+1]		<< '\t'
						<<      coord[i][Z+1]		<< '\t'
						<<      coord[i][ROLL+1]	<< '\t'
						<<      coord[i][PITCH+1]	<< '\t'
						<<      coord[i][YAW+1]
						<<		endl;
	}
	return true;
}