#ifndef _NAVIGATOR_H
#define _NAVIGATOR_H

#include "../../stdafx.h"

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

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

protected:
	uint16 correctionCountdown;
	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 RotateTo(float newAngle,float roationSpeed);

	void Rotate(float rotationSpeed);

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

	void CalculateDirection();

	void CalculateEngines();

	bool HasTarget()
	{
		return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
	}

	bool TargetReached();
	bool AngleReached();
};

#endif