using namespace RLGPC; // RLGymPPO
using namespace RLGSC; // RLGymSim

static std::vector<std::string> g_RewardNames = {};
MetricLoggerSystem g_MetricLoggerSystem;


void OnStep(GameInst* gameInst, const RLGSC::Gym::StepResult& stepResult, Report& gameMetrics) {
	if (auto* combRewards = dynamic_cast<LogCombinedReward*>(gameInst->match->rewardFn)) {
		for (size_t i = 0; i < combRewards->rewardFuncs.size(); i++) {
			float reward = combRewards->lastRewards[i] * combRewards->rewardWeights[i];
			gameMetrics.AccumAvg(combRewards->GetRewardFuncName(i), reward);
		}
	}

	// Fix: Pass stepResult directly instead of stepResult.state
	g_MetricLoggerSystem.LogStep(gameInst, stepResult, gameMetrics);

	// **Update the ball prediction here as well**
	BallPred::Update(gameInst->gym->arena);
}


void OnIteration(Learner* learner, Report& allMetrics) {
	auto allGameMetrics = learner->GetAllGameMetrics();
	g_MetricLoggerSystem.SummarizeIteration(allGameMetrics, allMetrics);

	// Handle rewards
	std::vector<AvgTracker> avgRews(g_RewardNames.size());
	for (size_t i = 0; i < g_RewardNames.size(); i++) {
		for (const auto& gameReport : allGameMetrics) {
			if (!gameReport.data.empty()) {
				avgRews[i] += gameReport.GetAvg(g_RewardNames[i]);
			}
		}
		allMetrics["Rewards/" + g_RewardNames[i]] = avgRews[i].Get();
	}
}




EnvCreateResult EnvCreateFunc() {
	constexpr int TICK_SKIP = 12;
	constexpr float NO_TOUCH_TIMEOUT_SECS = 8.f;
	int team_size = 1;
	std::cout << "Team Size: " << team_size << std::endl;

	const auto PZSUM = [](RewardFunction* rewardFunc, float teamSpirit = 1.0f, float opponentScale = 1.0f) {
		return new ZeroSumReward(rewardFunc, teamSpirit, opponentScale);
		};

	auto obs = new DefaultOBS();
	auto actionParser = new DiscreteAction();
	auto stateSetter = new WeightedSampleSetter(
		{
			{new RandomState(true, true, false), 0.9f},
			{new KickoffState(), 0.1f},
			{new AirDribbleSetup(), 0.0f},
			{new ReplaySetter(team_size), 0.0f},
		}
		);

	std::vector<TerminalCondition*> terminalConditions = {
		new NoTouchCondition(NO_TOUCH_TIMEOUT_SECS * 120 / TICK_SKIP),
		new GoalScoreCondition()
	};

	// Create a temporary reward function
	RewardFunction* tempReward;

	Match* match = new Match(
		tempReward,
		terminalConditions,
		obs,
		actionParser,
		stateSetter,
		team_size,
		true
	);

	Gym* gym = new Gym(match, TICK_SKIP);
	Arena* arena = gym->arena;



	// ======================== REWARDS ============================//
	
	auto rewards = new LogCombinedReward(
		std::vector<std::pair<RewardFunction*, float>>{

			{ PZSUM(new PossessionReward(), 0.0f, 0.25f), 1.0f },
	}
	);

	// Replace the temporary reward function with the actual one
	delete match->rewardFn;
	match->rewardFn = rewards;

	{
		static std::once_flag onceFlag;
		std::call_once(onceFlag, [&] {
			for (int i = 0; i < rewards->rewardFuncs.size(); i++)
				g_RewardNames.push_back(rewards->GetRewardFuncName(i));
			});
	}

	return { match, gym };
}

int main(int argc, char* argv[]) {
	// Initialize RocketSim with collision meshes
	RocketSim::Init("./collision_meshes");


	// Make configuration for the learner
	LearnerConfig cfg = {};
	cfg.renderMode = false;

	for (int i = 1; i < argc; ++i) {
		if (std::strcmp(argv[i], "--render") == 0) {
			cfg.renderMode = true;
			cfg.renderTimeScale = 1.0f;
			cfg.deterministic = true;
		}
	}

	// Play around with these to see what the optimal is for your machine, more isn't always better
	cfg.numThreads = 16;
	cfg.numGamesPerThread = 24;

	// We want a large itr/batch size
	// You'll want to increase this as your bot improves, up to an extent
	int tsPerItr = 100 * 1000;
	cfg.timestepsPerIteration = tsPerItr;
	cfg.ppo.batchSize = 100000;
	cfg.ppo.miniBatchSize = 25000; // Lower this if too much VRAM is being allocated
	cfg.expBufferSize = tsPerItr * 2;

	// This is just set to 1 to match rlgym-ppo example
	// I've found the best value is somewhere between 2 and 4
	// Increasing this will lower SPS, but increase step efficiency
	cfg.ppo.epochs = 3;

	// Reasonable starting entropy
	cfg.ppo.entCoef = 0.01f;

	cfg.skillTrackerConfig.enabled = true;
	cfg.timestepsPerSave = 2'000'000;
	cfg.checkpointsToKeep = 30;
	cfg.skillTrackerConfig.numEnvs = 16;
	cfg.skillTrackerConfig.numThreads = 16;
	cfg.skillTrackerConfig.perModeRatings = true;

	cfg.metricsRunName = "RUN6";
	cfg.metricsProjectName = "MAIN2";
	cfg.metricsGroupName = "RUN";


	// Good learning rate to start, may start to be too high around 100m steps
	cfg.ppo.policyLR = 2e-4; // 1e-4 -> 8e-5 -> 4e-5 -> 2e-5 ->1e-5
	cfg.ppo.criticLR = 2e-4;

	// Default model size
	cfg.ppo.policyLayerSizes = { 512, 512, 512 };
	cfg.ppo.criticLayerSizes = { 512, 512, 512 }; // tomorrow 1548, 1024, 512, 512, 256

	//cfg.ppo.policyLayerSizes = { 1024, 1024, 1024, 1024, 1024 };
	//cfg.ppo.criticLayerSizes = { 1024, 1024, 1024, 1024, 1024 };

	cfg.sendMetrics = true; // Send metrics

	// Make the learner with the environment creation function and the config we just made
	Learner learner = Learner(EnvCreateFunc, cfg);

	learner.stepCallback = OnStep;
	learner.iterationCallback = OnIteration;

	// Start learning!
	learner.Learn();

	return 0;
} #include <vector>
#include <memory>
#include <mutex>

// Include necessary project-specific headers
#include "C:\Epsilon\RLGymPPO_CPP-main\RLGymPPO_CPP\RLGymSim_CPP\RocketSim\src\Sim\Arena\Arena.h"
#include "C:\Epsilon\RLGymPPO_CPP-main\RLGymPPO_CPP\RLGymSim_CPP\src\RLGymSim_CPP\Utils\GlobalBallPred\BallPredTrackerManager.h"
#include "C:\Epsilon\RLGymPPO_CPP-main\RLGymPPO_CPP\RLGymSim_CPP\src\RLGymSim_CPP\Utils\Gamestates\PhysObj.h"



RS_NS_START

struct BallPredEnv {
    BallPredTracker ballPredTracker;

    BallPredEnv(const Arena* arena) :
        ballPredTracker(const_cast<Arena*>(arena), 240) {}

    void Update(const Arena* arena) {
        ballPredTracker.UpdatePredFromArena(const_cast<Arena*>(arena));
    }

    PhysObj GetAtTime(float time, const PhysObj& curBall);
};

class BallPred {
public:
    static BallPredEnv* GetOrMake(const Arena* arena);

    static PhysObj GetAtTime(const Arena* arena, float time, const PhysObj& curBall) {
        return GetOrMake(arena)->GetAtTime(time, curBall);
    }

    static void Update(const Arena* arena) {
        GetOrMake(arena)->Update(arena);
    }
};

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

		virtual float GetReward(const PlayerData& player, const GameState& state, const Action& prevAction) override {
			// The GameState should have a reference to the current Arena
			Arena* arena = state.lastArena;
			if (arena == nullptr) {
				RG_LOG("PossessionReward: Arena is null. Cannot predict ball state.");
				return 0.0f;
			}

			// Calculate the time for the player to reach the ball
			float timeToReach = TimeToReachCalculator::calculateTimeToReach(player, state.ball.pos);

			// Use the ball prediction system
			PhysObj predictedBallState = BallPred::GetAtTime(arena, timeToReach, state.ball);

			// Use the predicted ball state for more accurate calculations if needed
			// For example, you might want to adjust the timeToReach based on the predicted ball position

			// Log the time to reach (implement RG_LOG as needed)
			RG_LOG("Time to reach: " << timeToReach);

			// Return the calculated time
			return timeToReach;
		}
	};


	class TouchHeightReward : public Reward class JumpTouchReward : public RewardFunction {
public:
	JumpTouchReward(float min_height = 200.0f, float exp = 1.0f)
		: min_height(min_height), exp(exp), div(std::pow(CommonValues::CEILING_Z / 2 - CommonValues::BALL_RADIUS, exp)) {}

	virtual void Reset(const GameState& initialState) override {
		ticks_until_next_reward = 0;
	}

	virtual float GetReward(const PlayerData& player, const GameState& state, const Action& prevAction) override {
		if (player.ballTouchedStep && player.carState.isOnGround && state.ball.pos[2] >= min_height && ticks_until_next_reward <= 0) {
			ticks_until_next_reward = 149;
			float reward = std::pow(std::min(state.ball.pos[2], CommonValues::CEILING_Z / 2) - CommonValues::BALL_RADIUS, exp) / div;
			if (reward > 0.05)
				return reward;
		}
		ticks_until_next_reward -= 1;
		return 0.0f;
	}

private:
	float min_height;
	float exp;
	float div;
	int ticks_until_next_reward = 0;
};

class THeight : public RewardFunction {
private:
	float weight;

public:
	THeight(float weight = 1.0f) : weight(weight) {}

	float GetReward(const PlayerData& player, const GameState& state, const Action& prevAction) override {
		if (player.ballTouchedStep && !player.carState.isOnGround) {
			float touchHeight = state.ball.pos.z / CommonValues::CEILING_Z;
			//RG_LOG(touchHeight);
			return weight * touchHeight;
		}
		return 0.0f;
	}
};

class TAccel : public RewardFunction {
private:
	float weight;
	Vec lastBallVelocity;

public:
	TAccel(float weight = 1.0f) : weight(weight), lastBallVelocity(Vec()) {}

	void Reset(const GameState& initialState) override {
		lastBallVelocity = initialState.ball.vel;
	}

	float GetReward(const PlayerData& player, const GameState& state, const Action& prevAction) override {
		Vec curr_vel = state.ball.vel;
		float reward = 0.0f;

		if (player.ballTouchedStep) {
			// Exactly matching the Python logic:
			// norm(curr_vel - last_vel) / CAR_MAX_SPEED
			reward = (curr_vel - lastBallVelocity).Length() / CommonValues::CAR_MAX_SPEED;
		}
		//RG_LOG(reward);
		lastBallVelocity = curr_vel;  // Update for next step
		return weight * reward;
	}
}; #pragma once
#include "../../BaseInc.h"
#include "../Car/Car.h"
#include "../Ball/Ball.h"
#include "../BoostPad/BoostPad.h"
#include "../CollisionMasks.h"

#include "../../CollisionMeshFile/CollisionMeshFile.h"
#include "../BoostPad/BoostPadGrid/BoostPadGrid.h"
#include "../SuspensionCollisionGrid/SuspensionCollisionGrid.h"
#include "../MutatorConfig/MutatorConfig.h"
#include "ArenaConfig/ArenaConfig.h"

#include "../../../libsrc/bullet3-3.24/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
#include "../../../libsrc/bullet3-3.24/BulletCollision/CollisionShapes/btStaticPlaneShape.h"
#include "../../../libsrc/bullet3-3.24/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "../../../libsrc/bullet3-3.24/BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "../../../libsrc/bullet3-3.24/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "../../../libsrc/bullet3-3.24/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "../../../libsrc/bullet3-3.24/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"

RS_NS_START

typedef std::function<void(class Arena* arena, Team scoringTeam, void* userInfo)> GoalScoreEventFn;
typedef std::function<void(class Arena* arena, Car* bumper, Car* victim, bool isDemo, void* userInfo)> CarBumpEventFn;

// The container for all game simulation
// Stores cars, the ball, all arena collisions, and manages the overall game state
class Arena {
public:

	GameMode gameMode;

	uint32_t _lastCarID = 0;
	std::unordered_set<Car*> _cars;
	bool ownsCars = true; // If true, deleting this arena instance deletes all cars

	std::unordered_map<uint32_t, Car*> _carIDMap;
	
	Ball* ball;
	bool ownsBall = true; // If true, deleting this arena instance deletes the ball
	
	std::vector<BoostPad*> _boostPads;
	bool ownsBoostPads = true; // If true, deleing this arena instance deletes all boost pads
	
	BoostPadGrid _boostPadGrid;

	SuspensionCollisionGrid _suspColGrid;

	MutatorConfig _mutatorConfig;

	const MutatorConfig& GetMutatorConfig() { return _mutatorConfig; }
	RSAPI void SetMutatorConfig(const MutatorConfig& mutatorConfig);

	// Time in seconds each tick (1/tickrate)
	float tickTime; 

	// Returns (1 / tickTime)
	float GetTickRate() const {
		return 1 / tickTime;
	}

	// Total ticks this arena instance has been simulated for, never resets
	uint64_t tickCount = 0;

	const std::unordered_set<Car*>& GetCars() { return _cars; }
	const std::vector<BoostPad*>& GetBoostPads() { return _boostPads; }

	// Returns true if added, false if car was already added
	bool _AddCarFromPtr(Car* car);
	RSAPI Car* AddCar(Team team, const CarConfig& config = CAR_CONFIG_OCTANE);

	// Returns false if the car ID was not found in the cars list
	RSAPI bool RemoveCar(uint32_t id);

	// Returns false if the car was not found in the cars list
	// NOTE: If the car was removed, the car will be freed and the pointer will be made invalid
	bool RemoveCar(Car* car) {
		return RemoveCar(car->id);
	}

	RSAPI Car* GetCar(uint32_t id);

	btDiscreteDynamicsWorld _bulletWorld;
	struct {
		btDefaultCollisionConfiguration collisionConfig;
		btCollisionDispatcher collisionDispatcher;
		btOverlappingPairCache* overlappingPairCache;
		btBroadphaseInterface* broadphase;
		btSequentialImpulseConstraintSolver constraintSolver;
	} _bulletWorldParams;

	btRigidBody* _worldCollisionRBs = NULL;
	size_t _worldCollisionRBAmount = 0;
	btBvhTriangleMeshShape* _worldCollisionBvhShapes = NULL;
	btStaticPlaneShape* _worldCollisionPlaneShapes = NULL;

	struct {
		GoalScoreEventFn func = NULL;
		void* userInfo = NULL;
	} _goalScoreCallback;
	RSAPI void SetGoalScoreCallback(GoalScoreEventFn callbackFn, void* userInfo = NULL);

	struct {
		CarBumpEventFn func = NULL;
		void* userInfo = NULL;
	} _carBumpCallback;
	RSAPI void SetCarBumpCallback(CarBumpEventFn callbackFn, void* userInfo = NULL);

	// NOTE: Arena should be destroyed after use
	RSAPI static Arena* Create(GameMode gameMode, const ArenaConfig& arenaConfig = {}, float tickRate = 120);
	
	// Serialize entire arena state including cars, ball, and boostpads
	RSAPI void Serialize(DataStreamOut& out) const;

	// Load new arena from serialized data
	RSAPI static Arena* DeserializeNew(DataStreamIn& in);

	Arena(const Arena& other) = delete; // No copy constructor, use Arena::Clone() instead
	Arena& operator =(const Arena& other) = delete; // No copy operator, use Arena::Clone() instead

	Arena(Arena&& other) = delete; // No move constructor
	Arena& operator =(Arena&& other) = delete; // No move operator

	// Get a deep copy of the arena
	RSAPI Arena* Clone(bool copyCallbacks);

	// NOTE: Car ID will not be restored
	RSAPI Car* DeserializeNewCar(DataStreamIn& in, Team team);

	// Simulate everything in the arena for a given number of ticks
	RSAPI void Step(int ticksToSimulate = 1);

	RSAPI void ResetToRandomKickoff(int seed = -1);

	// Returns true if the ball is probably going in, does not account for wall or ceiling bounces
	// NOTE: Purposefully overestimates, just like the real RL's shot prediction
	// To check which goal it will score in, use the ball's velocity
	// Margin can be manually adjusted with extraMargin (negative to prevent overestimating)
	RSAPI bool IsBallProbablyGoingIn(float maxTime = 2.f, float extraMargin = 0, Team* goalTeamOut = NULL) const;

	// Returns true if the ball is in the net
	// Works for all gamemodes (and does nothing in THE_VOID)
	RSAPI bool IsBallScored() const;

	// Free all associated memory
	RSAPI ~Arena();

	// NOTE: Passed shape pointer will be freed when arena is deconstructed
	template <class T>
	btRigidBody* _AddStaticCollisionShape(
		size_t rbIndex, size_t meshListIndex, T* shape, T* meshList, btVector3 posBT = btVector3(0, 0, 0), 
		bool isHoopsNet = false) {

		static_assert(std::is_base_of<btCollisionShape, T>::value);
		meshList[meshListIndex] = *shape;

		assert(rbIndex < _worldCollisionRBAmount);
		btRigidBody& shapeRB = _worldCollisionRBs[rbIndex];
		shapeRB = btRigidBody(0, NULL, &meshList[meshListIndex]);
		shapeRB.setWorldTransform(btTransform(btMatrix3x3::getIdentity(), posBT));
		shapeRB.setUserPointer(this);
		if (isHoopsNet) {
			_bulletWorld.addRigidBody(&shapeRB, CollisionMasks::HOOPS_NET, CollisionMasks::HOOPS_NET);
		} else {
			_bulletWorld.addRigidBody(&shapeRB);
		}
		return &shapeRB;
	}

	void _SetupArenaCollisionShapes();

	// Static function called by Bullet internally when adding a collision point
	static bool _BulletContactAddedCallback(
		btManifoldPoint& cp,
		const btCollisionObjectWrapper* colObjA, int partID_A, int indexA,
		const btCollisionObjectWrapper* colObjB, int partID_B, int indexB
	);

	void _BtCallback_OnCarBallCollision(Car* car, Ball* ball, btManifoldPoint& manifoldPoint, bool ballIsBodyA);
	void _BtCallback_OnCarCarCollision(Car* car1, Car* car2, btManifoldPoint& manifoldPoint);
	void _BtCallback_OnCarWorldCollision(Car* car, btCollisionObject* worldObject, btManifoldPoint& manifoldPoint);

	const ArenaConfig& GetArenaConfig() const {
		return _config;
	}

	// Backwards compatability
	ArenaMemWeightMode GetMemWeightMode() {
		return _config.memWeightMode;
	}

private:
	
	// Constructor for use by Arena::Create()
	Arena(GameMode gameMode, const ArenaConfig& config, float tickRate = 120);

	// Making this private because horrible memory overflows can happen if you changed it
	ArenaConfig _config;
};

RS_NS_END Make it soo that the posession reward doesnt have to have any arena contructor while still working