#pragma once

#include "BasicSystem.h"

#include "OnlineGraph.h"

#include <queue>

#include <vector>

#include <deque>

#include <list>

#include <tuple>

#include <unordered_set>

#include <string>

class OnlineSystem : public BasicSystem

{

public:

OnlineSystem(const OnlineGrid& G, MAPFSolver& solver);

~OnlineSystem();

void simulate(int simulation_time);

// Assign one task row from the pool to the given agent.

// 'allow_cell' (if >= 0) means the candidate target is allowed to be exactly that cell

// (e.g., the agent's current cell when idling/holding).

bool assign_next_csv_task(size_t agent, int allow_cell = -1);

// Load tasks from a CSV file (implemented in step ③).

bool load_tasks_csv(const std::string& path);

// Add online constraints to the solver (e.g., yielding/time-shift constraints).

void add_online_constraints(std::list<std::tuple<int,int,int>>& cons) const override;

private:

const OnlineGrid& G;

// Store the paths of agents that have reached their goal locations.

std::list<Path> finished_paths;

// Set of currently occupied/locked goal cells.

std::unordered_set<int> busy_goals;

void move();

void save_results();

// Number of robots is fixed: this function only refreshes starts; it does NOT add new robots.

void update_start_and_goal_locations(int num_of_new_agents);

// Arrival-triggered switching (Start→Goal) / On completion→grab next task.

void update_goal_locations();

// ---- One row in the CSV task list ----

struct CSVTask {

int agent_id = -1;

int current = -1; // start cell (if empty, assign an entry cell)

int g1 = -1; // first-stage target

int g2 = -1; // second-stage target

int release_t = 0; // currently unused

};

// State needed for initialization & runtime

std::vector<CSVTask> csv_tasks; // tasks loaded from CSV

std::vector<int> cur_phase; // 0: ToStart (g1), 1: ToGoal (g2), 2: Idle

std::vector<int> cur_row; // index of the bound CSV row for each agent

// [ADD] Counters/statistics (initialized in simulate)

long long tasks_total = 0; // total number of CSV tasks

long long tasks_assigned = 0; // assigned (includes completed)

long long tasks_finished = 0; // completed

std::vector<int> per_agent_done; // completed count per agent

enum Phase { ToStart = 0, ToGoal = 1, Idle = 2 };

// Task pool: stores the row indices of tasks not yet assigned.

std::deque<int> csv_pool;

};