//////////////////////////////////////////////////////////////////////
// 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 "traject.h"
//#include "../include/traject/traject.h"

Coordinate Coordinate::temp;


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

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

	return *this;
}

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

Trajectory::Trajectory(int numOfCoords)
{
	try {
		coord = new Coordinate[numOfCoords];
		numcoords = numOfCoords;
	} catch (bad_alloc) {
		numcoords = 0;
	}

}


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

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

}

bool Trajectory::allocate(int newNumCoords)
{
	if( coord != NULL ) delete[] coord;
	numcoords = 0;

	try {
		coord = new Coordinate[newNumCoords];
		numcoords = newNumCoords;
		return true;
	} catch (bad_alloc) {
		return false;
	}
}

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

	try {
		if( coord != NULL ) delete[] coord;
		coord = new Coordinate[MAX_POINTS];
	} catch (bad_alloc) {
		return *(Trajectory*)NULL;
	}

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

	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][DT]	<< '\t'// time
						<<      coord[i][X]		<< '\t'
						<<      coord[i][Y]		<< '\t'
						<<      coord[i][Z]		<< '\t'
						<<      coord[i][ROLL]	<< '\t'
						<<      coord[i][PITCH]	<< '\t'
						<<      coord[i][YAW]
						<<		endl;
	}
	return true;
}


////////////////////////////////////////////////////////////
Coordinate& Coordinate::operator +(Coordinate c)
{
	temp.coordinate[DT] = coordinate[DT];
	for( int i = X; i < COORD_SIZE; ++i )
		temp.coordinate[i] = coordinate[i] + c.coordinate[i];
	return temp; 
}

Coordinate& Coordinate::operator -(Coordinate c)
{
	temp.coordinate[DT] = coordinate[DT];
	for( int i = X; i < COORD_SIZE; ++i )
		temp.coordinate[i] = coordinate[i] - c.coordinate[i];
	return temp; 
}

Coordinate& Coordinate::operator *(double d)
{
	temp.coordinate[DT] = coordinate[DT];
	for( int i = X; i < COORD_SIZE; ++i )
		temp.coordinate[i] = d*coordinate[i];
	return temp; 
}
Coordinate& Coordinate::operator /(double d)
{
	temp.coordinate[DT] = coordinate[DT];
	for( int i = X; i < COORD_SIZE; ++i )
		temp.coordinate[i] = coordinate[i] / d;
	return temp; 
}
Coordinate& Coordinate::operator =(double d)
{
	temp.coordinate[DT] = d;
	for( int i = X; i < COORD_SIZE; ++i )
		coordinate[i] = d;
	return *this; 
}

Coordinate& operator *(double d, Coordinate c)
{
	return c*d;
}

Coordinate::Coordinate(	double val)
{
	for( int i = 0; i < COORD_SIZE; ++i )
		coordinate[i] = val;
}
Coordinate::Coordinate(	double val[COORD_SIZE])
{
	for( int i = 0; i < COORD_SIZE; ++i )
		coordinate[i] = val[i];
}
Coordinate::Coordinate(	double deltaT
			, 	double x, double y, double z	// translation
			, 	double r, double p, double w )  // roll-pitch-yaw
{
	coordinate[ DT ] = deltaT;
	coordinate[ X  ] = x;
	coordinate[ Y  ] = y;
	coordinate[ Z  ] = z;
	coordinate[ROLL] = r;
	coordinate[PITCH]= p;
	coordinate[YAW ] = y;
}	 


//  example how to use class Coordinate
void coordinateExample(void)
{
	Coordinate c(0.1, 1., 2., 3., 1., 2., 3.); 
	double x = c[1];
	c[1] = 2*x;
	c[2] += c[3];

	Coordinate d;
	d = 2.0*c + c;

	d = d*2.0 + c*3.0;	//ERROR: two multiplications and one addition
					// require two temporary variables. Class Coordinate
					// supports only one temporary variable
}