#include "Position.h"

namespace cosi {

std::string Position::sQuantityName="Position";
std::string Position::sQuantityType="(Real,Real,Real)";

Position::Position() {
	mPosition.resize(3) ;
}

Position::Position(double pX, double pY, double pZ) {
	mPosition.resize(3) ;
	mPosition[0] = pX;
	mPosition[1] = pY;
	mPosition[2] = pZ;
	IsBot(false) ;
}

Position::~Position() {
}

Position Position::Get() {
	return (*this);
}

void Position::Set(Position pP) {
	IsBot(false);
	(*this) = pP ;
}

std::vector<double> Position::GetValue() {
	return mPosition ;
}

double Position::GetX() const {
	return mPosition[0];
}

double Position::GetY() const {
	return mPosition[1];
}

double Position::GetZ() const {
	return mPosition[2];
}

bool Position::operator ==(Position pP) {
	return ( GetValue() == pP.GetValue());
}

bool Position::operator<(const Position &pP) const {
	if( IsBot() & ! pP.IsBot() ) {
		return true ;
	}
	if ( !IsAny() && pP.IsAny()) {
		return true ;
	}
	return false ;
}

Position Position::operator+(Position pP) {
	Position P;
	if ( IsBot() ) {
		return pP;
	}
	if ( pP.IsBot()) {
		return Get();
	}
	if ( ! (Get() == pP.Get()) || IsAny() || pP.IsAny() ) {
		P.IsAny(true);
		return P;
	}
	//they are equal
	return pP;
}

Position Position::operator-(Position pP) {
	if ( IsAny() )
	return pP;
	if ( pP.IsAny())
	return Get();
	if ( Get() == pP.Get() ) {
		return pP;
	}
	return Position();
}

std::ostream& operator <<(std::ostream &pOs, const Position &pP) {
	pOs << "(" << pP.GetX() << "," << pP.GetY() << "," << pP.GetZ() << ")";
	return pOs;
}

double Norm1(Position P1 , Position P2) {
	double RetVal;
	RetVal = 0;
	RetVal+= fabs(P1.GetX() - P2.GetX() );
	RetVal+= fabs(P1.GetY() - P2.GetY() );
	RetVal+= fabs(P1.GetZ() - P2.GetZ() );
	return RetVal;
}

double Norm2(Position P1 , Position P2) {
	double RetVal;
	RetVal = 0;
	RetVal+= (P1.GetX() - P2.GetX())*(P1.GetX() - P2.GetX());
	RetVal+= (P1.GetY() - P2.GetY())*(P1.GetY() - P2.GetY());
	RetVal+= (P1.GetZ() - P2.GetZ())*(P1.GetZ() - P2.GetZ());
	return sqrt(RetVal);
}

}
