#include "LYGBasePlane.h"
#include "LYGConversion.h"


LYGBasePlane::LYGBasePlane()
	:m_Pos(LYGPoint(0,0)),m_DestinationPos(LYGPoint(100,100))
{
	m_MotionParam = boost::make_shared<MotionParam>();
}


LYGBasePlane::~LYGBasePlane()
{
}

bool LYGBasePlane::operator == (const LYGBasePlane & other)
{
	if (m_Name == other.m_Name)
	{
		return true;
	}
	return false;
}

void LYGBasePlane::addSensor(LYGBaseSensorPtr sensor)
{
	m_SensorVec.push_back(sensor);
}

std::vector<LYGBaseSensorPtr> LYGBasePlane::getSensors()
{
	return m_SensorVec;
}

void LYGBasePlane::setPos(LYGPoint pos)
{
	m_Pos = pos;
}

LYGPoint LYGBasePlane::getPos()
{
	return m_Pos;
}

void LYGBasePlane::setName(std::string name)
{
	m_Name = name;
}

std::string LYGBasePlane::getName()
{
	return m_Name;
}

void LYGBasePlane::setDestinationPos(LYGPoint destinationPos)
{
	m_DestinationPos = destinationPos;
}

LYGPoint LYGBasePlane::getDestinationPos()
{
	return m_DestinationPos;
}

void LYGBasePlane::fly()
{
	LYGPoint dir = getDestinationPos() - getPos();
	double head = 0.0;
	double pitch = 0.0;
	LYGConversion::instance()->ComputeHeadPitch(dir, getPos(), head, pitch);
	double newX = m_Pos.GetX() + m_MotionParam->speed * cos(head);
	m_Pos.SetX(newX);
	double newY = m_Pos.GetY() + m_MotionParam->speed * sin(head);
	m_Pos.SetY(newY);
}

void LYGBasePlane::start()
{

}