GameObject* S013010C_Aaron_Smith_Steering::GetClosestObject(Vector2D ahead1, Vector2D ahead2)
{
	GameObject* closestObj = nullptr;

	obstacles = ObstacleManager::Instance()->GetObstacles();
	for (GameObject* obj : obstacles)
	{
		//bool collision = CheckLineInstersect(ahead1, ahead2, obj);
		
		bool collision = Collisions::Instance()->PointInBox(ahead2, obj->GetAdjustedBoundingBox());

		if (collision)
		{
		
			if (closestObj == NULL)
				closestObj = obj;
			else
			{
				if (GetDistance(mTank->GetCentrePosition(), obj->GetCentralPosition()) < GetDistance(mTank->GetCentrePosition(), closestObj->GetCentralPosition()))
					closestObj = obj;
			}
		}
		
	}

	return closestObj;

}
Vector2D S013010C_Aaron_Smith_Steering::ObstacleAvoidance()
{
	bool safe = true;
	//Vector2D leftAhead1 = mTank->GetLeftAhead1();
	//Vector2D leftAhead2 = mTank->GetLeftAhead2();

	//Vector2D RightAhead1 = mTank->GetRightAhead1();
	//Vector2D RightAhead2 = mTank->GetRightAhead2();

	//Vector2D Ahead1 = mTank->GetAhead1();
	//Vector2D Ahead2 = mTank->GetAhead2();

	Vector2D leftAhead1 = mTank->mLeftFeelers[0];
	Vector2D leftAhead2 = mTank->mLeftFeelers[1];

	Vector2D RightAhead1 = mTank->mRightFeelers[0];
	Vector2D RightAhead2 = mTank->mRightFeelers[1];

	Vector2D Ahead1 = mTank->mStraightFeelers[0];
	Vector2D Ahead2 = mTank->mStraightFeelers[1];

	Vector2D rightSide1 = mTank->mRightSideFeelers[0];
	Vector2D rightSide2 = mTank->mRightSideFeelers[1];

	Vector2D leftSide1 = mTank->mLeftSideFeelers[0];
	Vector2D leftSide2 = mTank->mLeftSideFeelers[1];

	double maxAvoidForce = 500;
	//now find most threatening obstacle
	/*GameObject* closestObj = GetClosestObject(leftAhead1, leftAhead2);*/
	
	//check left feeler
	//check straight feeler
	//check right feeler
	GameObject* closestLeftObj = GetClosestObject(leftAhead1, leftAhead2);
	GameObject* closestStraightObj = GetClosestObject(Ahead1, Ahead2);
	GameObject* closestRightObj = GetClosestObject(RightAhead1, RightAhead2);
	GameObject* closestLeftSideObj = GetClosestObject(leftSide1, leftSide2);
	GameObject* closestRightSideObj = GetClosestObject(rightSide1, rightSide2);
	/*
	then compare the distance of them to find closest one
	*/

	double leftDistance = 999;
	double straightDistance = 999;
	double rightDistance = 999;
	double leftSideDistance = 999;
	double rightSideDistance = 999;
	
	if(closestLeftObj != nullptr)
		leftDistance = GetDistance(closestLeftObj->GetCentralPosition(), mTank->GetCentrePosition());
	if (closestStraightObj != nullptr)
		straightDistance = GetDistance(closestStraightObj->GetCentralPosition(), mTank->GetCentrePosition());
	if (closestRightObj != nullptr)
		rightDistance = GetDistance(closestRightObj->GetCentralPosition(), mTank->GetCentrePosition());
	if (closestLeftSideObj != nullptr)
		leftSideDistance = GetDistance(closestLeftSideObj->GetCentralPosition(), mTank->GetCentrePosition());
	if (closestRightSideObj != nullptr)
		rightSideDistance = GetDistance(closestRightSideObj->GetCentralPosition(), mTank->GetCentrePosition());

	Vector2D avoidForce;
	Vector2D totalForce = Vector2D(0,0);
	if (closestLeftObj != nullptr)
	{
		
		avoidForce = leftAhead1 - closestLeftObj->GetCentralPosition();
		avoidForce = Vec2DNormalize(avoidForce);
		avoidForce *= maxAvoidForce;
		totalForce += avoidForce;
	}

	if (closestRightObj != nullptr)
	{
		avoidForce = RightAhead1 - closestRightObj->GetCentralPosition();
		avoidForce = Vec2DNormalize(avoidForce);
		avoidForce *= maxAvoidForce;
		totalForce += avoidForce;
	}

	if (closestStraightObj != nullptr)
	{
		avoidForce = Ahead1 - closestStraightObj->GetCentralPosition();
		avoidForce = Vec2DNormalize(avoidForce);
		avoidForce *= maxAvoidForce;
		totalForce += avoidForce;
	}
	if (closestLeftSideObj != nullptr)
	{
		avoidForce = leftSide1 - closestLeftSideObj->GetCentralPosition();
		avoidForce = Vec2DNormalize(avoidForce);
		avoidForce *= maxAvoidForce;
		totalForce += avoidForce;
	}
	if (closestRightSideObj != nullptr)
	{
		avoidForce = rightSide1 - closestRightSideObj->GetCentralPosition();
		avoidForce = Vec2DNormalize(avoidForce);
		avoidForce *= maxAvoidForce;
		totalForce += avoidForce;
	}

	return totalForce;



}