src/lib/utils/robotOptimizations.ts

/**
 * Advanced Robot Optimization Utilities
 * Inspired by lerobot techniques for high-performance robot control
 */

import { getSafetyConfig, getTimingConfig, getLoggingConfig } from "$lib/configs/performanceConfig";
import type { RobotCommand } from "$lib/types/robotDriver";

/**
 * Performance timing utilities (inspired by lerobot's perf_counter usage)
 */
export class PerformanceTimer {
	private startTime: number;
	private logs: Map<string, number> = new Map();

	constructor() {
		this.startTime = performance.now();
	}

	/**
	 * Mark a timing checkpoint
	 */
	mark(label: string): void {
		const now = performance.now();
		this.logs.set(label, now - this.startTime);
		this.startTime = now;
	}

	/**
	 * Get timing for a specific label
	 */
	getTime(label: string): number | undefined {
		return this.logs.get(label);
	}

	/**
	 * Get all timing data
	 */
	getAllTimings(): Map<string, number> {
		return new Map(this.logs);
	}

	/**
	 * Log performance data in lerobot style
	 */
	logPerformance(prefix: string = "robot"): void {
		if (!getLoggingConfig().ENABLE_TIMING_MEASUREMENTS) return;

		const items: string[] = [];
		for (const [label, timeMs] of this.logs) {
			const hz = 1000 / timeMs;
			items.push(`${label}:${timeMs.toFixed(2)}ms (${hz.toFixed(1)}Hz)`);
		}

		if (items.length > 0) {
			console.log(`${prefix} ${items.join(" ")}`);
		}
	}

	/**
	 * Reset timing data
	 */
	reset(): void {
		this.logs.clear();
		this.startTime = performance.now();
	}
}

/**
 * Safety position clamping (inspired by lerobot's ensure_safe_goal_position)
 * Prevents sudden large movements that could damage the robot
 */
export function ensureSafeGoalPosition(
	goalPosition: number,
	currentPosition: number,
	maxRelativeTarget?: number
): number {
	const safetyConfig = getSafetyConfig();

	if (!safetyConfig.ENABLE_POSITION_CLAMPING) {
		return goalPosition;
	}

	const maxTarget = maxRelativeTarget || safetyConfig.MAX_RELATIVE_TARGET_DEG;
	const deltaPosition = goalPosition - currentPosition;

	// Check for emergency stop condition
	if (Math.abs(deltaPosition) > safetyConfig.EMERGENCY_STOP_THRESHOLD_DEG) {
		console.warn(
			`⚠️ Emergency stop: movement too large (${deltaPosition.toFixed(1)}° > ${safetyConfig.EMERGENCY_STOP_THRESHOLD_DEG}°)`
		);
		return currentPosition; // Don't move at all
	}

	// Clamp to maximum relative movement
	if (Math.abs(deltaPosition) > maxTarget) {
		const clampedPosition = currentPosition + Math.sign(deltaPosition) * maxTarget;
		console.log(
			`🛡️ Safety clamp: ${goalPosition.toFixed(1)}° → ${clampedPosition.toFixed(1)}° (max: ±${maxTarget}°)`
		);
		return clampedPosition;
	}

	return goalPosition;
}

/**
 * Velocity limiting (inspired by lerobot's joint velocity constraints)
 */
export function limitJointVelocity(
	currentPosition: number,
	goalPosition: number,
	deltaTimeMs: number,
	maxVelocityDegS?: number
): number {
	const safetyConfig = getSafetyConfig();
	const maxVel = maxVelocityDegS || safetyConfig.MAX_JOINT_VELOCITY_DEG_S;

	const deltaPosition = goalPosition - currentPosition;
	const deltaTimeS = deltaTimeMs / 1000;
	const requiredVelocity = Math.abs(deltaPosition) / deltaTimeS;

	if (requiredVelocity > maxVel) {
		const maxMovement = maxVel * deltaTimeS;
		const limitedPosition = currentPosition + Math.sign(deltaPosition) * maxMovement;

		console.log(
			`🐌 Velocity limit: ${requiredVelocity.toFixed(1)}°/s → ${maxVel}°/s (pos: ${limitedPosition.toFixed(1)}°)`
		);

		return limitedPosition;
	}

	return goalPosition;
}

/**
 * Busy wait for precise timing (inspired by lerobot's busy_wait function)
 * More accurate than setTimeout for high-frequency control loops
 */
export async function busyWait(durationMs: number): Promise<void> {
	const timingConfig = getTimingConfig();

	if (!timingConfig.USE_BUSY_WAIT || durationMs <= 0) {
		return;
	}

	const startTime = performance.now();
	const targetTime = startTime + durationMs;

	// Use a combination of setTimeout and busy waiting for efficiency
	if (durationMs > 5) {
		// For longer waits, use setTimeout for most of the duration
		await new Promise((resolve) => setTimeout(resolve, durationMs - 2));
	}

	// Busy wait for the remaining time for high precision
	while (performance.now() < targetTime) {
		// Busy loop - more accurate than setTimeout for short durations
	}
}

/**
 * Frame rate controller with precise timing
 */
export class FrameRateController {
	private lastFrameTime: number;
	private targetFrameTimeMs: number;
	private frameCount: number = 0;
	private performanceTimer: PerformanceTimer;

	constructor(targetFps: number) {
		this.targetFrameTimeMs = 1000 / targetFps;
		this.lastFrameTime = performance.now();
		this.performanceTimer = new PerformanceTimer();
	}

	/**
	 * Wait until the next frame should start
	 */
	async waitForNextFrame(): Promise<void> {
		const now = performance.now();
		const elapsed = now - this.lastFrameTime;
		const remaining = this.targetFrameTimeMs - elapsed;

		if (remaining > 0) {
			await busyWait(remaining);
		}

		this.lastFrameTime = performance.now();
		this.frameCount++;

		// Log performance periodically
		if (this.frameCount % 60 === 0) {
			const actualFrameTime = now - this.lastFrameTime + remaining;
			const actualFps = 1000 / actualFrameTime;
			this.performanceTimer.logPerformance(`frame_rate: ${actualFps.toFixed(1)}fps`);
		}
	}

	/**
	 * Get current frame timing info
	 */
	getFrameInfo(): { frameCount: number; actualFps: number; targetFps: number } {
		const now = performance.now();
		const actualFrameTime = now - this.lastFrameTime;
		const actualFps = 1000 / actualFrameTime;
		const targetFps = 1000 / this.targetFrameTimeMs;

		return {
			frameCount: this.frameCount,
			actualFps,
			targetFps
		};
	}
}

/**
 * Batch command processor for efficient joint updates
 */
export class BatchCommandProcessor {
	private commandQueue: RobotCommand[] = [];
	private batchSize: number;
	private processingInterval?: number;

	constructor(batchSize: number = 10, processingIntervalMs: number = 16) {
		this.batchSize = batchSize;
		this.startProcessing(processingIntervalMs);
	}

	/**
	 * Add a command to the batch queue
	 */
	queueCommand(command: RobotCommand): void {
		this.commandQueue.push(command);

		// Process immediately if batch is full
		if (this.commandQueue.length >= this.batchSize) {
			this.processBatch();
		}
	}

	/**
	 * Process a batch of commands
	 */
	private processBatch(): RobotCommand[] {
		if (this.commandQueue.length === 0) return [];

		const batch = this.commandQueue.splice(0, this.batchSize);

		// Merge commands for the same joints (latest wins)
		const mergedJoints = new Map<string, { name: string; value: number }>();

		for (const command of batch) {
			for (const joint of command.joints) {
				mergedJoints.set(joint.name, joint);
			}
		}

		const mergedCommand: RobotCommand = {
			timestamp: Date.now(),
			joints: Array.from(mergedJoints.values()),
			metadata: { source: "batch_processor", batchSize: batch.length }
		};

		return [mergedCommand];
	}

	/**
	 * Start automatic batch processing
	 */
	private startProcessing(intervalMs: number): void {
		this.processingInterval = setInterval(() => {
			if (this.commandQueue.length > 0) {
				this.processBatch();
			}
		}, intervalMs);
	}

	/**
	 * Stop batch processing
	 */
	stop(): void {
		if (this.processingInterval) {
			clearInterval(this.processingInterval);
			this.processingInterval = undefined;
		}
	}

	/**
	 * Get queue status
	 */
	getQueueStatus(): { queueLength: number; batchSize: number } {
		return {
			queueLength: this.commandQueue.length,
			batchSize: this.batchSize
		};
	}
}

/**
 * Connection health monitor
 */
export class ConnectionHealthMonitor {
	private lastSuccessTime: number;
	private errorCount: number = 0;
	private healthCheckInterval?: number;

	constructor(healthCheckIntervalMs: number = 1000) {
		this.lastSuccessTime = Date.now();
		this.startHealthCheck(healthCheckIntervalMs);
	}

	/**
	 * Report a successful operation
	 */
	reportSuccess(): void {
		this.lastSuccessTime = Date.now();
		this.errorCount = 0;
	}

	/**
	 * Report an error
	 */
	reportError(): void {
		this.errorCount++;
	}

	/**
	 * Check if connection is healthy
	 */
	isHealthy(maxErrorCount: number = 5, maxSilenceMs: number = 5000): boolean {
		const timeSinceLastSuccess = Date.now() - this.lastSuccessTime;
		return this.errorCount < maxErrorCount && timeSinceLastSuccess < maxSilenceMs;
	}

	/**
	 * Get health metrics
	 */
	getHealthMetrics(): { errorCount: number; timeSinceLastSuccessMs: number; isHealthy: boolean } {
		return {
			errorCount: this.errorCount,
			timeSinceLastSuccessMs: Date.now() - this.lastSuccessTime,
			isHealthy: this.isHealthy()
		};
	}

	/**
	 * Start health monitoring
	 */
	private startHealthCheck(intervalMs: number): void {
		this.healthCheckInterval = setInterval(() => {
			const metrics = this.getHealthMetrics();
			if (!metrics.isHealthy) {
				console.warn(
					`⚠️ Connection health warning: ${metrics.errorCount} errors, ${metrics.timeSinceLastSuccessMs}ms since last success`
				);
			}
		}, intervalMs);
	}

	/**
	 * Stop health monitoring
	 */
	stop(): void {
		if (this.healthCheckInterval) {
			clearInterval(this.healthCheckInterval);
			this.healthCheckInterval = undefined;
		}
	}
}

/**
 * Async operation timeout utility
 */
export function withTimeout<T>(
	promise: Promise<T>,
	timeoutMs: number,
	operation: string = "operation"
): Promise<T> {
	return Promise.race([
		promise,
		new Promise<never>((_, reject) =>
			setTimeout(() => reject(new Error(`${operation} timed out after ${timeoutMs}ms`)), timeoutMs)
		)
	]);
}