#include<cmath>
#include<string>

class point_and_direction {
	double x, y, z; // Y is up like math class.  Like unity but not like unreal
	double heading; // 0 is toward lower numbers on the X axis, in radians
	double elevation; // 0 is flat
public:
	point_and_direction() {}

	point_and_direction(double ix, double iy, double iz, double ih, double ie) 
		: x(ix), y(iy), z(iz), heading(ih), elevation(ie) 
	{}

	void turn(double howfar){
		heading += howfar;
		// Or:
		// this->heading += howfar;
	}
	void elevate(double howfar){
		elevation += howfar;
	}
	void forward(double distance) {
		x += distance * sin(heading);
		z += distance * cos(heading);
	}
	point_and_direction operator+(const point_and_direction& b){
		point_and_direction c;
		c.x = (this->x + b.x) / 2;
		c.y = (y + b.y) / 2;
		c.z = (z + b.z) / 2;
		c.heading = (heading + b.heading) / 2;
		c.elevation = (elevation + b.elevation) / 2;
		return c;
	}
	std::string to_string() const {
		return "(" + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(z) + 
			", " + std::to_string(heading) + ", " + std::to_string(elevation) + ")";
	}
	friend std::ostream& operator<< (std::ostream& out, const point_and_direction& tout); // not needed in this case
};

#include<iostream>

std::ostream& operator<< (std::ostream& out, const point_and_direction& tout){
	return out << tout.to_string();
}

#include<stdio.h>
using namespace std;
// silly, so get rid of this at some point:
#define PI 3.141592653
int main(){
	point_and_direction testpos(3, 3, 2, PI/4, PI/12);
	puts(testpos.to_string().c_str());
	testpos.forward(10);
	puts(testpos.to_string().c_str());
	testpos.turn(PI/2);
	testpos.forward(10);
	puts(testpos.to_string().c_str());
	cout << testpos << endl; 
	point_and_direction testpos2(1, 1, 1, PI, PI/2);
	cout << testpos + testpos2 << endl;

}