// include/telemetry.cpp
// Telemetry server implementation for broadcasting car telemetry data to connected clients.
#include "telemetry.h"

std::vector<int> telemetry_clients;
pthread_mutex_t clients_mutex = PTHREAD_MUTEX_INITIALIZER;

// Helper struct for sorting cars by position
struct CarPosition {
	int index;
	u_int16_t laps;
	float splinePos;

	bool operator<(const CarPosition &other) const {
		// First compare by laps (more laps = better position)
		if (laps != other.laps) {
			return laps > other.laps;
		}
		// If same laps, compare by spline position (further along = better)
		return splinePos > other.splinePos;
	}
};

// Broadcast telemetry to all connected clients
void broadcast_telemetry(const api_packet &packet) {
	telemetry_packet telem;
	telem.server_id = packet.tracker_id;
	telem.car_count = 0;

	// Temporary storage for connected cars
	std::vector<CarPosition> positions;
	std::vector<int> carIndices; // Maps telem index to original packet index

	// First pass: collect all connected cars
	for (int i = 0; i < MAX_PLAYERS; i++) {
		if (packet.cars[i].isConnected) {
			CarPosition pos;
			pos.index = telem.car_count;
			pos.laps = packet.cars[i].total_laps_completed;
			pos.splinePos = packet.cars[i].normalizedSplinePos;

			positions.push_back(pos);
			carIndices.push_back(i);

			auto &car = telem.cars[telem.car_count];

			car.carID = packet.cars[i].carID;
			strncpy(car.driver_name, packet.cars[i].driver_name, sizeof(car.driver_name) - 1);
			car.driver_name[sizeof(car.driver_name) - 1] = '\0';
			strncpy(car.driver_guid, packet.cars[i].driver_GUID, sizeof(car.driver_guid) - 1);
			car.driver_guid[sizeof(car.driver_guid) - 1] = '\0';
			strncpy(car.car_model, packet.cars[i].car_model, sizeof(car.car_model) - 1);
			car.car_model[sizeof(car.car_model) - 1] = '\0';

			car.position.x = packet.cars[i].position.x;
			car.position.y = packet.cars[i].position.y;
			car.position.z = packet.cars[i].position.z;

			car.normalizedSplinePos = packet.cars[i].normalizedSplinePos;

			// Calculate speed from velocity vector (m/s to km/h)
			float speed_ms = std::sqrt(std::pow(packet.cars[i].velocity.x, 2.0f) + std::pow(packet.cars[i].velocity.y, 2.0f) + std::pow(packet.cars[i].velocity.z, 2.0f));
			car.speed_kmh = speed_ms * 3.6f;

			car.gear = packet.cars[i].carGear;
			car.rpm = packet.cars[i].carRPM;
			car.last_lap_time = packet.cars[i].lap_time;
			car.best_lap_time = 0; // TODO: Track best lap across session
			car.current_lap = packet.cars[i].total_laps_completed;

			telem.car_count++;
		}
	}

	// Sort cars by position
	std::sort(positions.begin(), positions.end());

	// Assign positions (1-indexed)
	for (size_t i = 0; i < positions.size(); i++) {
		telem.cars[positions[i].index].position_rank = i + 1;
	}

	// Broadcast to all connected clients
	pthread_mutex_lock(&clients_mutex);

	// Calculate actual packet size (header + only connected cars)
	size_t header_size = 2; // server_id + car_count
	size_t car_size = sizeof(telemetry_packet::car_telemetry);
	size_t packet_size = header_size + (telem.car_count * car_size);

	for (auto it = telemetry_clients.begin(); it != telemetry_clients.end();) {
		ssize_t sent = send(*it, &telem, packet_size, MSG_NOSIGNAL);

		if (sent < 0) {
			// Client disconnected, remove from list
			printf("[T] Telemetry client %d disconnected\n", *it);
			close(*it);
			it = telemetry_clients.erase(it);
		} else {
			++it;
		}
	}

	pthread_mutex_unlock(&clients_mutex);
}

// Thread to accept telemetry client connections
void *telemetry_server_thread(void *arg) {
	(void)arg;

	int server_fd = socket(AF_UNIX, SOCK_STREAM, 0);
	if (server_fd < 0) {
		perror("[!] Telemetry socket creation failed");
		return NULL;
	}

	unlink(TELEMETRY_SOCKET_PATH);

	struct sockaddr_un addr;
	addr.sun_family = AF_UNIX;
	strncpy(addr.sun_path, TELEMETRY_SOCKET_PATH, sizeof(addr.sun_path) - 1);
	addr.sun_path[sizeof(addr.sun_path) - 1] = '\0';

	if (bind(server_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		perror("[!] Telemetry socket bind failed");
		close(server_fd);
		return NULL;
	}

	if (listen(server_fd, 5) < 0) {
		perror("[!] Telemetry socket listen failed");
		close(server_fd);
		return NULL;
	}

	printf("[+] Telemetry server listening on %s\n", TELEMETRY_SOCKET_PATH);

	while (1) {
		int client_fd = accept(server_fd, NULL, NULL);
		if (client_fd < 0) {
			perror("[!] Telemetry accept failed");
			continue;
		}

		pthread_mutex_lock(&clients_mutex);

		if (telemetry_clients.size() < (size_t)MAX_TELEMETRY_CLIENTS) {
			telemetry_clients.push_back(client_fd);
			printf("[+] Telemetry client connected (total: %zu)\n", telemetry_clients.size());
		} else {
			printf("[!] Max telemetry clients reached, rejecting connection\n");
			close(client_fd);
		}

		pthread_mutex_unlock(&clients_mutex);
	}

	close(server_fd);
	return NULL;
}