Vector4 SteeringBehavior::evade(Handle &p_pursuer_handle){
	IHasHandle* tmp;
	MovingObject* pursuer;
	Vector4 toPursuer;
	float look_ahead_time;

	tmp = theWorld.get(p_pursuer_handle);
	if (pursuer = dynamic_cast<MovingObject*>(tmp)){
		toPursuer = pursuer->getPosition() - owner->getPosition();
		look_ahead_time = toPursuer.length() / (owner->getMaxSpeed() + pursuer->getSpeed());
		return flee(pursuer->getPosition() + pursuer->getVelocity() * look_ahead_time);
	}
	else {
		off(BehaviorType::evade);
	}
	return Vector4();
}
Vector4 SteeringBehavior::offsetPursuit(Handle &p_leader, Vector4 &offset) {
	MovingObject* leader = dynamic_cast<MovingObject*>(theWorld.get(p_leader));

	if (leader == nullptr) {
		// Leader does not exist anymore
		off(BehaviorType::offset_pursuit);
		return Vector4(0, 0, 0);
	}

	// calculate the offset's position in world space
	Vector4 WorldOffsetPos = leader->getPosition() + offset;

	Vector4 ToOffset = WorldOffsetPos - owner->getPosition();

	// The lookahead time is propotional to the distance between the leader
	// and the pursuer; and is inversely proportional to the sum of both
	// agent's velocities
	float LookAheadTime = ToOffset.length() / (owner->getMaxSpeed() + leader->getSpeed());

	// Arrive at the predicted future position of the offset
	return arrive(WorldOffsetPos + leader->getVelocity() * LookAheadTime, fast);
}
Vector4 SteeringBehavior::pursuit(Handle &p_evader_handle){
	IHasHandle* tmp;
	MovingObject* evader;
	Vector4 toEvader;
	float relativeHeading, look_ahead_time;

	tmp = theWorld.get(p_evader_handle);
	if (evader = dynamic_cast<MovingObject*>(tmp)){
		toEvader = evader->getPosition() - owner->getPosition();
		relativeHeading = evader->getHeading().dot(owner->getHeading());

		if (toEvader.dot(owner->getHeading()) > 0 && relativeHeading < -0.95){
			return this->seek(evader->getPosition());
		}

		look_ahead_time = toEvader.length() / (owner->getMaxSpeed() + evader->getSpeed());
		return seek(evader->getPosition() + evader->getVelocity() * look_ahead_time);
	}
	else {
		off(BehaviorType::pursuit);
	}
	return Vector4(0, 0, 0);
}