//-----------------------------------------------------------------------------
// Purpose: Test for func_orientation_volume ents which could effect the placement angles of a portal.
// Input  : vecCurAngles - Default angles to place (may change)
//			vecCurOrigin - origin of the portal on placement
//			pPortal - The portal attempting to place
// Output : Returns true on success, false on failure.
//-----------------------------------------------------------------------------
bool UTIL_TestForOrientationVolumes( QAngle& vecCurAngles, const Vector& vecCurOrigin, const CProp_Portal* pPortal )
{
    if ( !pPortal )
        return false;

    // Walk list of orientation volumes, obb test each with candidate portal
    CFuncPortalOrientation *pList = g_FuncPortalOrientationVolumeList.m_pClassList;
    while ( pList )
    {
        if ( !pList->IsActive() )
        {
            pList = pList->m_pNext;
            continue;
        }

        if ( IsOBBIntersectingOBB( vecCurOrigin, vecCurAngles, CProp_Portal_Shared::vLocalMins, CProp_Portal_Shared::vLocalMaxs,
                                   pList->GetAbsOrigin(), pList->GetCollideable()->GetCollisionAngles(), pList->GetCollideable()->OBBMins(), pList->GetCollideable()->OBBMaxs() ) )
        {
            QAngle vecGoalAngles;
            // Ent is marked to match angles of it's linked partner
            if ( pList->m_bMatchLinkedAngles )
            {
                // This feature requires a linked portal on a floor or ceiling. Bail without effecting
                // the placement angles if we fail those requirements.
                CProp_Portal* pLinked = pPortal->m_hLinkedPortal.Get();
                if ( !pLinked || !(AnglesAreEqual( vecCurAngles.x, -90.0f, 0.1f ) || AnglesAreEqual( vecCurAngles.x, 90.0f, 0.1f )) )
                    return false;

                vecGoalAngles = pLinked->GetAbsAngles();
                vecCurAngles.y = 0.0f;
                vecCurAngles.z = vecGoalAngles.z;
            }
            // Match the angles loaded in from the map
            else
            {
                vecGoalAngles = pList->m_vecAnglesToFace;
                vecCurAngles = vecGoalAngles;
            }

            return true;
        }
        pList = pList->m_pNext;
    }

    return false;
}
// Navigate our current ladder. Return true if we are doing ladder navigation.
// TODO: Need Push() and Pop() for run/walk context to keep ladder speed contained.
bool CCSBot::UpdateLadderMovement()
{
	if (!m_pathLadder)
		return false;

	bool giveUp = false;

	// check for timeout
	const float ladderTimeoutDuration = 10.0f;
	if (gpGlobals->time - m_pathLadderTimestamp > ladderTimeoutDuration)
	{
		PrintIfWatched("Ladder timeout!\n");
		giveUp = true;
	}

	else if (m_pathLadderState == APPROACH_ASCENDING_LADDER
		|| m_pathLadderState == APPROACH_DESCENDING_LADDER
		|| m_pathLadderState == ASCEND_LADDER
		|| m_pathLadderState == DESCEND_LADDER
		|| m_pathLadderState == DISMOUNT_ASCENDING_LADDER
		|| m_pathLadderState == MOVE_TO_DESTINATION)
	{
		if (m_isStuck)
		{
			PrintIfWatched("Giving up ladder - stuck\n");
			giveUp = true;
		}
	}

	if (giveUp)
	{
		// jump off ladder and give up
		Jump(MUST_JUMP);
		Wiggle();
		ResetStuckMonitor();
		DestroyPath();
		Run();
		return false;
	}

	ResetStuckMonitor();

	// check if somehow we totally missed the ladder
	switch (m_pathLadderState)
	{
	case MOUNT_ASCENDING_LADDER:
	case MOUNT_DESCENDING_LADDER:
	case ASCEND_LADDER:
	case DESCEND_LADDER:
	{
		const float farAway = 200.0f;
		Vector2D d = (m_pathLadder->m_top - pev->origin).Make2D();
		if (d.IsLengthGreaterThan(farAway))
		{
			PrintIfWatched("Missed ladder\n");
			Jump(MUST_JUMP);
			DestroyPath();
			Run();
			return false;
		}
		break;
	}
	}

	m_areaEnteredTimestamp = gpGlobals->time;

	const float tolerance = 10.0f;
	const float closeToGoal = 25.0f;

	switch (m_pathLadderState)
	{
	case APPROACH_ASCENDING_LADDER:
	{
		bool approached = false;
		Vector2D d(pev->origin.x - m_goalPosition.x, pev->origin.y - m_goalPosition.y);

		if (d.x * m_pathLadder->m_dirVector.x + d.y * m_pathLadder->m_dirVector.y < 0.0f)
		{
			Vector2D perp(-m_pathLadder->m_dirVector.y, m_pathLadder->m_dirVector.x);

#ifdef REGAMEDLL_FIXES
			if (Q_abs(d.x * perp.x + d.y * perp.y) < tolerance && d.Length() < closeToGoal)
#else
			if (Q_abs(int64(d.x * perp.x + d.y * perp.y)) < tolerance && d.Length() < closeToGoal)
#endif
				approached = true;
		}

		// small radius will just slow them down a little for more accuracy in hitting their spot
		const float walkRange = 50.0f;
		if (d.IsLengthLessThan(walkRange))
		{
			Walk();
			StandUp();
		}

		// TODO: Check that we are on the ladder we think we are
		if (IsOnLadder())
		{
			m_pathLadderState = ASCEND_LADDER;
			PrintIfWatched("ASCEND_LADDER\n");

			// find actual top in case m_pathLadder penetrates the ceiling
			ComputeLadderEndpoint(true);
		}
		else if (approached)
		{
			// face the m_pathLadder
			m_pathLadderState = FACE_ASCENDING_LADDER;
			PrintIfWatched("FACE_ASCENDING_LADDER\n");
		}
		else
		{
			// move toward ladder mount point
			MoveTowardsPosition(&m_goalPosition);
		}
		break;
	}
	case APPROACH_DESCENDING_LADDER:
	{
		// fall check
		if (GetFeetZ() <= m_pathLadder->m_bottom.z + HalfHumanHeight)
		{
			PrintIfWatched("Fell from ladder.\n");

			m_pathLadderState = MOVE_TO_DESTINATION;
			m_path[m_pathIndex].area->GetClosestPointOnArea(&m_pathLadder->m_bottom, &m_goalPosition);

			AddDirectionVector(&m_goalPosition, m_pathLadder->m_dir, HalfHumanWidth);
			PrintIfWatched("MOVE_TO_DESTINATION\n");
		}
		else
		{
			bool approached = false;
			Vector2D d(pev->origin.x - m_goalPosition.x, pev->origin.y - m_goalPosition.y);

			if (d.x * m_pathLadder->m_dirVector.x + d.y * m_pathLadder->m_dirVector.y > 0.0f)
			{
				Vector2D perp(-m_pathLadder->m_dirVector.y, m_pathLadder->m_dirVector.x);

				if (Q_abs(int64(d.x * perp.x + d.y * perp.y)) < tolerance && d.Length() < closeToGoal)
					approached = true;
			}

			// if approaching ladder from the side or "ahead", walk
			if (m_pathLadder->m_topBehindArea != m_lastKnownArea)
			{
				const float walkRange = 150.0f;
				if (!IsCrouching() && d.IsLengthLessThan(walkRange))
					Walk();
			}

			// TODO: Check that we are on the ladder we think we are
			if (IsOnLadder())
			{
				// we slipped onto the ladder - climb it
				m_pathLadderState = DESCEND_LADDER;
				Run();
				PrintIfWatched("DESCEND_LADDER\n");

				// find actual bottom in case m_pathLadder penetrates the floor
				ComputeLadderEndpoint(false);
			}
			else if (approached)
			{
				// face the ladder
				m_pathLadderState = FACE_DESCENDING_LADDER;
				PrintIfWatched("FACE_DESCENDING_LADDER\n");
			}
			else
			{
				// move toward ladder mount point
				MoveTowardsPosition(&m_goalPosition);
			}
		}
		break;
	}
	case FACE_ASCENDING_LADDER:
	{
		// find yaw to directly aim at ladder
		Vector to = m_pathLadder->m_bottom - pev->origin;
		Vector idealAngle = UTIL_VecToAngles(to);

		const float angleTolerance = 5.0f;
		if (AnglesAreEqual(pev->v_angle.y, idealAngle.y, angleTolerance))
		{
			// move toward ladder until we become "on" it
			Run();
			ResetStuckMonitor();
			m_pathLadderState = MOUNT_ASCENDING_LADDER;
			PrintIfWatched("MOUNT_ASCENDING_LADDER\n");
		}
		break;
	}
	case FACE_DESCENDING_LADDER:
	{
		// find yaw to directly aim at ladder
		Vector to = m_pathLadder->m_top - pev->origin;
		Vector idealAngle = UTIL_VecToAngles(to);

		const float angleTolerance = 5.0f;
		if (AnglesAreEqual(pev->v_angle.y, idealAngle.y, angleTolerance))
		{
			// move toward ladder until we become "on" it
			m_pathLadderState = MOUNT_DESCENDING_LADDER;
			ResetStuckMonitor();
			PrintIfWatched("MOUNT_DESCENDING_LADDER\n");
		}
		break;
	}
	case MOUNT_ASCENDING_LADDER:
	{
		if (IsOnLadder())
		{
			m_pathLadderState = ASCEND_LADDER;
			PrintIfWatched("ASCEND_LADDER\n");

			// find actual top in case m_pathLadder penetrates the ceiling
			ComputeLadderEndpoint(true);
		}

		MoveForward();
		break;
	}
	case MOUNT_DESCENDING_LADDER:
	{
		// fall check
		if (GetFeetZ() <= m_pathLadder->m_bottom.z + HalfHumanHeight)
		{
			PrintIfWatched("Fell from ladder.\n");

			m_pathLadderState = MOVE_TO_DESTINATION;
			m_path[m_pathIndex].area->GetClosestPointOnArea(&m_pathLadder->m_bottom, &m_goalPosition);

			AddDirectionVector(&m_goalPosition, m_pathLadder->m_dir, HalfHumanWidth);
			PrintIfWatched("MOVE_TO_DESTINATION\n");
		}
		else
		{
			if (IsOnLadder())
			{
				m_pathLadderState = DESCEND_LADDER;
				PrintIfWatched("DESCEND_LADDER\n");

				// find actual bottom in case m_pathLadder penetrates the floor
				ComputeLadderEndpoint(false);
			}

			// move toward ladder mount point
			MoveForward();
		}
		break;
	}
	case ASCEND_LADDER:
	{
		// run, so we can make our dismount jump to the side, if necessary
		Run();

		// if our destination area requires us to crouch, do it
		if (m_path[m_pathIndex].area->GetAttributes() & NAV_CROUCH)
			Crouch();

		// did we reach the top?
		if (GetFeetZ() >= m_pathLadderEnd)
		{
			// we reached the top - dismount
			m_pathLadderState = DISMOUNT_ASCENDING_LADDER;
			PrintIfWatched("DISMOUNT_ASCENDING_LADDER\n");

			if (m_path[m_pathIndex].area == m_pathLadder->m_topForwardArea)
				m_pathLadderDismountDir = FORWARD;
			else if (m_path[m_pathIndex].area == m_pathLadder->m_topLeftArea)
				m_pathLadderDismountDir = LEFT;
			else if (m_path[m_pathIndex].area == m_pathLadder->m_topRightArea)
				m_pathLadderDismountDir = RIGHT;

			m_pathLadderDismountTimestamp = gpGlobals->time;
		}
		else if (!IsOnLadder())
		{
			// we fall off the ladder, repath
			DestroyPath();
			return false;
		}

		// move up ladder
		MoveForward();
		break;
	}
	case DESCEND_LADDER:
	{
		Run();

		float destHeight = m_pathLadderEnd + HalfHumanHeight;
		if (!IsOnLadder() || GetFeetZ() <= destHeight)
		{
			// we reached the bottom, or we fell off - dismount
			m_pathLadderState = MOVE_TO_DESTINATION;
			m_path[m_pathIndex].area->GetClosestPointOnArea(&m_pathLadder->m_bottom, &m_goalPosition);

			AddDirectionVector(&m_goalPosition, m_pathLadder->m_dir, HalfHumanWidth);
			PrintIfWatched("MOVE_TO_DESTINATION\n");
		}

		// Move down ladder
		MoveForward();
		break;
	}
	case DISMOUNT_ASCENDING_LADDER:
	{
		if (gpGlobals->time - m_pathLadderDismountTimestamp >= 0.4f)
		{
			m_pathLadderState = MOVE_TO_DESTINATION;
			m_path[m_pathIndex].area->GetClosestPointOnArea(&pev->origin, &m_goalPosition);
			PrintIfWatched("MOVE_TO_DESTINATION\n");
		}

		// We should already be facing the dismount point
		if (m_pathLadderFaceIn)
		{
			switch (m_pathLadderDismountDir)
			{
			case LEFT:    StrafeLeft(); break;
			case RIGHT:   StrafeRight(); break;
			case FORWARD: MoveForward(); break;
			}
		}
		else
		{
			switch (m_pathLadderDismountDir)
			{
			case LEFT:    StrafeRight();  break;
			case RIGHT:   StrafeLeft();   break;
			case FORWARD: MoveBackward(); break;
			}
		}
		break;
	}
	case MOVE_TO_DESTINATION:
	{
		if (m_path[m_pathIndex].area->Contains(&pev->origin))
		{
			// successfully traversed ladder and reached destination area
			// exit ladder state machine
			PrintIfWatched("Ladder traversed.\n");
			m_pathLadder = nullptr;

			// incrememnt path index to next step beyond this ladder
			SetPathIndex(m_pathIndex + 1);

			return false;
		}

		MoveTowardsPosition(&m_goalPosition);
		break;
	}
	}

	return true;
}