class PossessionReward : public RewardFunction {
public:
	PossessionReward() = default;

	virtual float GetReward(const PlayerData& player, const GameState& state, const Action& prevAction) override {
		// Define prediction parameters
		constexpr float PREDICTION_DURATION = 2.0f; // 6 seconds of prediction
		constexpr int NUM_PREDICTION_POINTS = 8;
		constexpr float PREDICTION_STEP = PREDICTION_DURATION / NUM_PREDICTION_POINTS; // Time interval between predictions

		// Find the opponent (if any)
		const PlayerData* opponent = nullptr;
		for (const auto& otherPlayer : state.players) {
			if (otherPlayer.team != player.team) {
				opponent = &otherPlayer;
				break;
			}
		}

		// Initialize possession counters
		float localPossession = 0.0f;
		float opponentPossession = 0.0f;

		// Loop through the prediction points
		for (int i = 0; i < NUM_PREDICTION_POINTS; ++i) {
			// Get predicted ball state at each prediction step
			float predictionTime = i * PREDICTION_STEP;
			PhysObj predictedBallState = BallPred::getInstance().GetAtTime(predictionTime);

			Vec ballPos(predictedBallState.pos.x, predictedBallState.pos.y, predictedBallState.pos.z);

			// Calculate player's time to reach the predicted ball position
			float localTimeToReach = TimeToReachCalculator::calculateTimeToReach(player, ballPos);

			// If opponent exists, calculate their time to reach as well
			if (opponent) {
				float opponentTimeToReach = TimeToReachCalculator::calculateTimeToReach(*opponent, ballPos);

				// Compare time to reach the ball and adjust possession
				if (localTimeToReach < opponentTimeToReach) {
					localPossession += 1.0f;
				}
				else {
					opponentPossession += 1.0f;
				}
			}
			else {
				// If no opponent, assume full possession for the local player
				localPossession += 1.0f;
			}
		}

		// Calculate total possession
		float totalPossession = localPossession + opponentPossession;
		localPossession /= totalPossession;  // Normalize possession to be between 0 and 1
		opponentPossession /= totalPossession;

		// Determine if player has possession
		bool hasPossession = localPossession > opponentPossession;

		// Only log possession for the blue team
		if (player.team == Team::BLUE) {
			RG_LOG("Blue team possession: " << (hasPossession ? "Yes" : "No"));
		}

		// Reward based on possession comparison
		float reward = (localPossession > opponentPossession) ? 1.0f : 0.0f;
		//RG_LOG("PossessionReward: Local possession: " << localPossession * 100 << "%, Reward: " << reward);
		return reward;
	}
}; class TimeToReachCalculator {
public:
	static float calculateTimeToReach(const PlayerData& player, const Vec& targetPos) {
		Vec carPos = player.phys.pos;
		Vec carVel = player.phys.vel;
		Vec dirToTarget = (targetPos - carPos).Normalized();
		float dist = (targetPos - carPos).Length() - CommonValues::BALL_RADIUS;
		float vel = carVel.Dot(dirToTarget);
		float rotTime = calculateRotationTime(player, targetPos);
		float timeToSurface = calculateTimeToReachAnySurface(player);

		float t = 0;
		for (float curDist = dist, curVel = vel; curDist > DIST_REACH_THRESH; t += STEP_INTERVAL, curDist -= curVel * STEP_INTERVAL) {
			bool hasRotated = t >= rotTime;
			bool onSurface = t >= timeToSurface;

			if (curVel < MAX_TOTAL_SPEED && t == 0) {
				// Flip force
				curVel += FLIP_FORCE;
			}

			if (onSurface) {
				if (curVel < 0) {
					curVel += (BRAKE_ACCEL + BOOST_ACCEL_GROUND * hasRotated) * STEP_INTERVAL;
				}
				else if (curVel < MAX_DRIVE_SPEED) {
					curVel += (DRIVE_ACCEL + BOOST_ACCEL_GROUND * hasRotated) * STEP_INTERVAL;
				}
				else if (curVel < MAX_TOTAL_SPEED) {
					curVel += BOOST_ACCEL_GROUND * hasRotated * STEP_INTERVAL;
				}
			}

			if (curVel >= MAX_TOTAL_SPEED) {
				return t + (curDist / curVel);
			}
		}
		return t;
	}

private:
	// Constants
	static constexpr float SIDE_WALL_X = 4096.0f;
	static constexpr float CLOSE_DISTANCE = 10.0f;
	static constexpr float NEAR_WALL_DISTANCE = 4077.0f;
	static constexpr float Z_THRESHOLD = 256.0f;
	static constexpr float EPSILON = 1e-3f;
	static constexpr float DIST_REACH_THRESH = 80.0f;
	static constexpr float STEP_INTERVAL = 1.0f / 60.0f;
	static constexpr float FLIP_FORCE = 495.0f;
	static constexpr float AIR_ACCEL = 66.66667f;
	static constexpr float BOOST_ACCEL_AIR = 1058.33333f;
	static constexpr float MAX_TOTAL_SPEED = 2300.0f;
	static constexpr float BRAKE_ACCEL = 3500.0f;
	static constexpr float BOOST_ACCEL_GROUND = 991.66667f;
	static constexpr float DRIVE_ACCEL = 1600.0f;
	static constexpr float MAX_DRIVE_SPEED = 1410.0f;
	static constexpr float GRAVITY_Z = -650.0f;
	static constexpr float BACK_WALL_Y = 5120.0f;
	static constexpr float NEG_BACK_WALL_Y = -5120.0f;
	static constexpr float MAX_BACKWALL_Y = 5100.0f;
	static constexpr float BOOST_ACCEL_POS = 1000.0f;
	static constexpr float BOOST_ACCEL_NEG = -1000.0f;
	static constexpr float FLOOR_Z = 18.0f;
	static constexpr float BOOST_ACCEL_DOWN = -1000.0f;

	static float calculateTimeToSideWalls(const PlayerData& player) {
		const Vec& pos = player.phys.pos;
		const Vec& vel = player.phys.vel;

		float minTime = std::numeric_limits<float>::infinity();

		if (std::abs(pos.x) >= NEAR_WALL_DISTANCE && std::abs(pos.x) <= SIDE_WALL_X) {
			return 0.0f;
		}

		if (vel.x > EPSILON) {
			float a = 0.5f * DRIVE_ACCEL;
			float b = vel.x;
			float c = pos.x - SIDE_WALL_X;

			float discriminant = b * b - 4.0f * a * c;

			if (discriminant >= 0.0f) {
				float sqrtD = std::sqrt(discriminant);
				float t1 = (-b + sqrtD) / (2.0f * a);
				float t2 = (-b - sqrtD) / (2.0f * a);

				if (t1 > EPSILON && t1 < minTime) {
					minTime = t1;
				}
				if (t2 > EPSILON && t2 < minTime) {
					minTime = t2;
				}
			}
		}

		if (vel.x < -EPSILON) {
			float a = 0.5f * (-DRIVE_ACCEL);
			float b = vel.x;
			float c = pos.x + SIDE_WALL_X;

			float discriminant = b * b - 4.0f * a * c;

			if (discriminant >= 0.0f) {
				float sqrtD = std::sqrt(discriminant);
				float t1 = (-b + sqrtD) / (2.0f * a);
				float t2 = (-b - sqrtD) / (2.0f * a);

				if (t1 > EPSILON && t1 < minTime) {
					minTime = t1;
				}
				if (t2 > EPSILON && t2 < minTime) {
					minTime = t2;
				}
			}
		}

		return minTime;
	}

	static float calculateTimeToBackWalls(const PlayerData& player) {
		const Vec& pos = player.phys.pos;
		const Vec& vel = player.phys.vel;

		float minTime = std::numeric_limits<float>::infinity();

		if (pos.y >= MAX_BACKWALL_Y || pos.y <= -MAX_BACKWALL_Y) {
			return 0.0f;
		}

		if (pos.y >= BACK_WALL_Y || pos.y <= NEG_BACK_WALL_Y) {
			return 0.0f;
		}

		if (vel.y > EPSILON) {
			float a = 0.5f * BOOST_ACCEL_POS;
			float b = vel.y;
			float c = pos.y - BACK_WALL_Y;

			float discriminant = b * b - 4.0f * a * c;

			if (discriminant >= 0.0f) {
				float sqrtD = std::sqrt(discriminant);
				float t1 = (-b + sqrtD) / (2.0f * a);
				float t2 = (-b - sqrtD) / (2.0f * a);

				if (t1 > EPSILON && t1 < minTime) {
					minTime = t1;
				}
				if (t2 > EPSILON && t2 < minTime) {
					minTime = t2;
				}
			}
		}

		if (vel.y < -EPSILON) {
			float a = 0.5f * BOOST_ACCEL_NEG;
			float b = vel.y;
			float c = pos.y - NEG_BACK_WALL_Y;

			float discriminant = b * b - 4.0f * a * c;

			if (discriminant >= 0.0f) {
				float sqrtD = std::sqrt(discriminant);
				float t1 = (-b + sqrtD) / (2.0f * a);
				float t2 = (-b - sqrtD) / (2.0f * a);

				if (t1 > EPSILON && t1 < minTime) {
					minTime = t1;
				}
				if (t2 > EPSILON && t2 < minTime) {
					minTime = t2;
				}
			}
		}

		return minTime;
	}

	static float calculateTimeToFloor(const PlayerData& player) {
		const Vec& pos = player.phys.pos;
		const Vec& vel = player.phys.vel;

		if (pos.z <= FLOOR_Z) {
			return 0.0f;
		}

		float totalAccel = GRAVITY_Z + BOOST_ACCEL_DOWN;

		float a = 0.5f * totalAccel;
		float b = vel.z;
		float c = pos.z - FLOOR_Z;

		float discriminant = b * b - 4.0f * a * c;

		if (discriminant < 0.0f) {
			return std::numeric_limits<float>::infinity();
		}

		float sqrtD = std::sqrt(discriminant);
		float t1 = (-b - sqrtD) / (2.0f * a);
		float t2 = (-b + sqrtD) / (2.0f * a);

		float timeToFloor = std::numeric_limits<float>::infinity();

		if (t1 > 0.0f) {
			timeToFloor = t1;
		}
		if (t2 > 0.0f && t2 < timeToFloor) {
			timeToFloor = t2;
		}

		return timeToFloor;
	}

	static float calculateTimeToReachAnySurface(const PlayerData& player) {
		// Calculate individual times to each surface
		float timeToSideWalls = calculateTimeToSideWalls(player);
		// RG_LOG(timeToSideWalls); // Uncomment if logging is needed

		float timeToBackWalls = calculateTimeToBackWalls(player);
		//RG_LOG(timeToBackWalls); // Uncomment if logging is needed

		float timeToFloor = calculateTimeToFloor(player);
		// RG_LOG(timeToFloor); // Uncomment if logging is needed

		// Assign identifier variables for clarity
		float minTimeBetweenBackWallsAndFloor = RS_MIN(timeToBackWalls, timeToFloor);

		// Now find the minimum time among side walls and the previously computed minimum
		float minTime = RS_MIN(timeToSideWalls, minTimeBetweenBackWallsAndFloor);

		return minTime;
	} 

	static float calculateRotationTime(const PlayerData& player, const Vec& targetPos) {
		constexpr float MAX_ANG_VEL = 5.5f * 0.9f;

		Vec playerPos = player.phys.pos;
		Vec playerForward = player.phys.rotMat.forward;
		Vec dirToTarget = (targetPos - playerPos).Normalized();
		float dotProduct = RS_CLAMP(playerForward.Dot(dirToTarget), -1.0f, 1.0f);
		float angleDiff = std::acos(dotProduct);

		return angleDiff / MAX_ANG_VEL;
	}
};
 Vec enemyGoalPos = player.team == Team::BLUE ? CommonValues::ORANGE_GOAL_BACK : CommonValues::BLUE_GOAL_BACK;
Vec ownGoalPos = player.team == Team::BLUE ? CommonValues::BLUE_GOAL_BACK : CommonValues::ORANGE_GOAL_BACK;
const auto& ball = state.ball;
Vec ballToGoalDir2D = ((enemyGoalPos - ball.pos) * Vec(1, 1, 0)).Normalized();
float speedToGoal = ball.vel.Dot(ballToGoalDir2D);
float ballFlatSpeed = (ball.vel * Vec(1, 1, 0)).Length();
float goalAlign = speedToGoal / RS_MAX(ballFlatSpeed, 0.01f);
Vec deltaToBall = ball.pos - player.phys.pos;
float distToBall = deltaToBall.Length();
Vec dirToBall = deltaToBall / RS_MAX(distToBall, 0.01f);
float dirToBallZ = dirToBall.z;
float speedToBall = player.phys.vel.Dot(dirToBall);
float relSpeedToBall = (player.phys.vel - ball.vel).Dot(dirToBall);
float distToGoal2D = ((enemyGoalPos - player.phys.pos) * Vec(1, 1, 0)).Length(); // New reward cutoff logic
float goalY = RLConst::SOCCAR_GOAL_SCORE_BASE_THRESHOLD_Y * (player.team == Team::BLUE ? 1 : -1);
float distToGoalY = std::abs(ball.pos.y - goalY);
float extrapScoreTime = distToGoalY / std::max(std::abs(ball.vel.y), 0.01f);
Vec extrapScorePos = ball.pos + ball.vel * extrapScoreTime;
extrapScorePos.z += CommonValues::GRAVITY_Z * (extrapScoreTime * extrapScoreTime) / 2;

if (extrapScoreTime <= MIN_SCORE_TIME || extrapScorePos.z >= MAX_EXTRAP_Z) {
				reward = 0.0f; // Disable reward if the ball is about to score or go too high
} now that i have a working possession system and time to reach calculation can you make an really accurate etimation for the min time the opponent possibly can score on the player