#ifndef _NAVIGATOR_H
#define _NAVIGATOR_H

//#include <math.h>
#include "../../stdafx.h"

class Navigator : public IO_Module
{
public:
	Navigator()
	{
		this->parent = NULL;
		this->moduleId = 0;
		this->direction = -1.0f;
		this->targetAngle = -1.0f;
		this->targetX = -1.0f;
		this->targetY = -1.0f;
		this->robotSpeed = 0;
		this->rotationSpeed = 0;
	}

	Navigator(uint32 navigatorId)
	{
		this->parent = NULL;
		this->moduleId = navigatorId;
		this->direction = -1.0f;
		this->targetAngle = -1.0f;
		this->targetX = -1.0f;
		this->targetY = -1.0f;
		this->robotSpeed = 0;
		this->rotationSpeed = 0;
	}

protected:
	float direction; 
	float targetAngle;
	float targetX;
	float targetY;
	float robotSpeed;
	float rotationSpeed;

public:
	void Update();	

	void Stop();

	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);

	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);

	void Rotate(float rotationSpeed);

	void SetSpeed(float newSpeed)
	{
		this->robotSpeed = newSpeed;
	}
};

#endif