#include <RLGymPPO_CPP/Learner.h>
#include "C:\Epsilon\RLGymPPO_CPP-main\RLGymPPO_CPP\RLGymSim_CPP\src\RLGymSim_CPP\Utils\GlobalBallPred.h"

// Rewards
#include <RLGymSim_CPP/Utils/RewardFunctions/EPSILON_LOGGED_REWARDS.h>
#include <RLGymSim_CPP/Utils/RewardFunctions/Rewards.h>

//++++++++++++++++++++++++++++++++++++++++++++++++++++//

// Loggers
#include "RLGymSim_CPP/Utils/Loggers/Loggers.h"

//++++++++++++++++++++++++++++++++++++++++++++++++++++//


// StateSetting
#include <RLGymSim_CPP/Utils/StateSetters/WeightedSampleSetter.h>


//++++++++++++++++++++++++++++++++++++++++++++++++++++//


// Observation Space

#define INCLUDE_OBS_IMPLEMENTATIONS
#include <RLGymSim_CPP/Utils/OBSBuilders/AllObservationBuilders.h>
#undef INCLUDE_OBS_IMPLEMENTATIONS

//++++++++++++++++++++++++++++++++++++++++++++++++++++//



// TerminalConditions
#define INCLUDE_Condition_IMPLEMENTATIONS
#include <RLGymSim_CPP/Utils/TerminalConditions/ALLTerminalConditions.h>
#undef INCLUDE_Condition_IMPLEMENTATIONS

//++++++++++++++++++++++++++++++++++++++++++++++++++++//


//  ActionParser
#include <RLGymSim_CPP/Utils/ActionParsers/DiscreteMirroredAction.h>

//++++++++++++++++++++++++++++++++++++++++++++++++++++//

#include "RLBotClient.h"

#include <\Epsilon\RLGymPPO_CPP-main\Rewards.h>

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);

}


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 = 16.f;

	int team_size = 1;
	//static int counter = 0;
	//int team_size = (rand() % 100 < 50) ? 1 : ((rand() % 100 < 90) ? 2 : 3);

	std::cout << "Team Size: " << team_size << std::endl;

	// { PZSUM(new Reward(), 1.0f, 0.75f), 1.0f},
		   // { new Reward(), 1.0f},

	// Define PZSUM as a local variable
	const auto PZSUM = [](RewardFunction* rewardFunc, float teamSpirit = 1.0f, float opponentScale = 1.0f) {
		return new ZeroSumReward(rewardFunc, teamSpirit, opponentScale);
		};


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

			{ new sBP1(arena), 1.0f },
			
	}
	);

	{
		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));
			}
		);
	}

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

	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},
		}
		);

	Match* match = new Match(
		rewards,
		terminalConditions,
		obs,
		actionParser,
		stateSetter,

		team_size, // Team size
		true // Spawn opponents
	);

	Gym* gym = new Gym(match, TICK_SKIP);
	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 = 20;

	// 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 = 1e-4; // 1e-4 -> 8e-5 -> 4e-5 -> 2e-5 ->1e-5
	cfg.ppo.criticLR = 1e-4;

	// Default model size
	cfg.ppo.policyLayerSizes = { 512, 512, 512, 512 };
	cfg.ppo.criticLayerSizes = { 512, 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);

	// Set up our callbacks
	learner.stepCallback = [](GameInst* gameInst, const RLGSC::Gym::StepResult& stepResult, Report& gameMetrics) {
		// Original OnStep logic
		OnStep(gameInst, stepResult, gameMetrics);

		// Update GlobalBallPred
		BallPred::Update(gameInst->gym->arena);
	};


	learner.iterationCallback = OnIteration;

	// Start learning!
	learner.Learn();

	return 0;
} give the reward the correct arena #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