#ifndef HEADING_H
#define HEADING_H

#include <cmath>
#include <random>

#include "Angle.h"

struct Heading {

#define _2PI (2*M_PI)

private:

	/** heading in radians. 0 = to-the-right */
	float rad;

public:

	/** ctor with radians */
	Heading(const float rad) : rad(rad) {
		Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
	}

	/** ctor from(x1,y1) to(x2,y2) */
	Heading(const float x1, const float y1, const float x2, const float y2) : rad(Angle::getRAD_2PI(x1,y1,x2,y2)) {
		Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
	}

	/** POSITIVE angular difference [0:PI] */
	float getDiffHalfRAD(const Heading other) const {
		return Angle::getDiffRAD_2PI_PI(rad, other.rad);
	}

	/** signled angular difference [-PI:+PI] */
	float getSignedDiff(const Heading other) const {
		return Angle::getSignedDiffRAD_2PI(rad, other.rad);
	}

	/** update the angle but ensure we stay within [0:2PI] */
	Heading& operator += (const float _rad) {
//		Assert::isBetween(_rad, float(-_2PI*0.999), float(+_2PI*0.999), "radians out of bounds");
//		Assert::isBetween(_rad, float(-_2PI), float(+_2PI), "radians out of bounds");
		rad += _rad;
		while	(rad >= _2PI)	{rad -= _2PI;}
		while	(rad < 0)		{rad += _2PI;}
		return *this;
	}

	/** update the angle but ensure we stay within [0:2PI] */
	Heading& operator -= (const float _rad) {
		return *this += (-_rad);
	}

	/** update the angle but ensure we stay within [0:2PI] */
	Heading operator + (const float _rad) const {
		return (Heading(*this) += _rad);
	}

	/** update the angle but ensure we stay within [0:2PI] */
	Heading operator - (const float _rad) const {
		return (Heading(*this) -= _rad);
	}


	/** update the angle but ensure we stay within [0:2PI] */
	Heading operator - (const Heading head) const {
		return *this - head.rad;
	}

	/** update the angle but ensure we stay within [0:2PI] */
	Heading operator + (const Heading head) const {
		return *this + head.rad;
	}


        Heading& operator = (const float _rad) {
            rad = _rad;
            return *this;
        }

	/** compare two headings */
	bool operator == (const Heading other) const {return rad == other.rad;}
	bool operator != (const Heading other) const {return rad != other.rad;}

	/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
	Heading getInverted() const {
		Heading out(rad);
		out += M_PI;
		return out;
	}

	float getRAD() const {return rad;}

//	/** get a random heading */
//	static Heading rnd() {
//		static std::minstd_rand gen(1234);
//		static std::uniform_real_distribution<float> dist(0, _2PI);
//		const float rnd = dist(gen);
//		return Heading(rnd);
//	}

#undef _2PI

};

namespace Headings {
	static const Heading RIGHT =	Heading(M_PI*0/2);
	static const Heading UP =		Heading(M_PI*1/2);
	static const Heading LEFT =		Heading(M_PI*2/2);
	static const Heading DOWN =		Heading(M_PI*3/2);
}

#endif // HEADING_H