#ifndef FRAME_H_INCLUDED
#define FRAME_H_INCLUDED 1

// Copyright (C) 2005 Zbynek Winkler, zw at robotika cz
// Licensed to the public under the terms of the GNU GPL 2

#include "point.h"

class frame
{
	public:
		point m_point;
		double m_heading;
	public:
		frame() : m_point(), m_heading() {}
		frame(const point & in_point, const double & in_heading) : m_point(in_point), m_heading(in_heading) {}
		frame(const double & in_x, const double & in_y, const double & in_heading) : m_point(in_x, in_y), m_heading(in_heading) {}
		
		double & x() { return m_point.m_x; }
		double & y() { return m_point.m_y; }
		double & heading() { return m_heading; }

		bool eq(const frame & in_p, const double & in_eps = 1e-4) const 
		{
			return m_point.eq(in_p.m_point, in_eps) && ::eq(m_heading, in_p.m_heading, in_eps);
		}
	
		frame& advanceBy(const double & in_f)
		{
			m_point.m_x += in_f * cos(m_heading);
			m_point.m_y += in_f * sin(m_heading);
			return *this;
		}
		frame& slideBy(const double & in_s)
		{
			m_point.m_x -= in_s * sin(m_heading);
			m_point.m_y += in_s * cos(m_heading);
			return *this;
		}
		frame& turnBy(const double & in_angle)
		{
			m_heading += in_angle;
			return *this;
		}
		frame& offsetBy(const double & in_f, const double & in_s)
		{
			advanceBy(in_f);
			slideBy(in_s);
			return *this;
		}
		frame& offsetBy(const point & in_off)
		{
			return offsetBy(in_off.m_x, in_off.m_y);
		}
		
		point transform(const point & in_p) const
		{
			return frame(*this).offsetBy(in_p).m_point;
		}
		
		point offsetTo(const point & in_p) const
		{ 
			point pom(in_p - m_point);
			double a = pom.angle() - m_heading;
			double d = pom.magnitude();
			return point(d * cos(a), d * sin(a)); 
		}
};

#endif