src/lib/robot/drivers/MockSequenceMaster.ts

import type {
	MasterDriver,
	ConnectionStatus,
	RobotCommand,
	CommandSequence,
	MockSequenceMasterConfig,
	CommandCallback,
	SequenceCallback,
	StatusChangeCallback,
	UnsubscribeFn
} from "$lib/types/robotDriver";
import { getRobotPollingConfig } from "$lib/configs/performanceConfig";

/**
 * Mock Sequence Master Driver
 * Provides predefined movement sequences for testing master-slave architecture
 */
export class MockSequenceMaster implements MasterDriver {
	readonly type = "master" as const;
	readonly id: string;
	readonly name: string;

	private _status: ConnectionStatus = { isConnected: false };
	private config: MockSequenceMasterConfig;

	// Event callbacks
	private commandCallbacks: CommandCallback[] = [];
	private sequenceCallbacks: SequenceCallback[] = [];
	private statusCallbacks: StatusChangeCallback[] = [];

	// Playback control
	private isPlaying = false;
	private isPaused = false;
	private currentSequenceIndex = 0;
	private currentCommandIndex = 0;
	private playbackIntervalId?: number;

	constructor(config: MockSequenceMasterConfig) {
		this.config = config;
		this.id = `mock-sequence-${Date.now()}`;
		this.name = `Mock Sequence Master`;

		console.log(`Created MockSequenceMaster with ${config.sequences.length} sequences`);
	}

	get status(): ConnectionStatus {
		return this._status;
	}

	async connect(): Promise<void> {
		console.log(`Connecting ${this.name}...`);

		this._status = { isConnected: true, lastConnected: new Date() };
		this.notifyStatusChange();

		// Auto-start if configured
		if (this.config.autoStart) {
			await this.start();
		}

		console.log(`${this.name} connected`);
	}

	async disconnect(): Promise<void> {
		console.log(`Disconnecting ${this.name}...`);

		await this.stop();
		this._status = { isConnected: false };
		this.notifyStatusChange();

		console.log(`${this.name} disconnected`);
	}

	async start(): Promise<void> {
		if (!this._status.isConnected) {
			throw new Error("Cannot start: master not connected");
		}

		if (this.isPlaying && !this.isPaused) {
			console.log("Sequence already playing");
			return;
		}

		console.log(`Starting sequence playback...`);
		this.isPlaying = true;
		this.isPaused = false;

		// Reset to beginning if not paused
		if (this.currentSequenceIndex >= this.config.sequences.length) {
			this.currentSequenceIndex = 0;
			this.currentCommandIndex = 0;
		}

		this.startPlaybackLoop();
	}

	async stop(): Promise<void> {
		console.log("Stopping sequence playback");

		this.isPlaying = false;
		this.isPaused = false;
		this.currentSequenceIndex = 0;
		this.currentCommandIndex = 0;

		if (this.playbackIntervalId) {
			clearInterval(this.playbackIntervalId);
			this.playbackIntervalId = undefined;
		}
	}

	async pause(): Promise<void> {
		console.log("Pausing sequence playback");
		this.isPaused = true;

		if (this.playbackIntervalId) {
			clearInterval(this.playbackIntervalId);
			this.playbackIntervalId = undefined;
		}
	}

	async resume(): Promise<void> {
		if (!this.isPlaying || !this.isPaused) {
			return;
		}

		console.log("Resuming sequence playback");
		this.isPaused = false;
		this.startPlaybackLoop();
	}

	// Event subscription methods
	onCommand(callback: CommandCallback): UnsubscribeFn {
		this.commandCallbacks.push(callback);
		return () => {
			const index = this.commandCallbacks.indexOf(callback);
			if (index >= 0) {
				this.commandCallbacks.splice(index, 1);
			}
		};
	}

	onSequence(callback: SequenceCallback): UnsubscribeFn {
		this.sequenceCallbacks.push(callback);
		return () => {
			const index = this.sequenceCallbacks.indexOf(callback);
			if (index >= 0) {
				this.sequenceCallbacks.splice(index, 1);
			}
		};
	}

	onStatusChange(callback: StatusChangeCallback): UnsubscribeFn {
		this.statusCallbacks.push(callback);
		return () => {
			const index = this.statusCallbacks.indexOf(callback);
			if (index >= 0) {
				this.statusCallbacks.splice(index, 1);
			}
		};
	}

	// Private methods
	private startPlaybackLoop(): void {
		if (this.playbackIntervalId) {
			clearInterval(this.playbackIntervalId);
		}

		this.playbackIntervalId = setInterval(() => {
			this.executeNextCommand();
		}, getRobotPollingConfig().SEQUENCE_PLAYBACK_INTERVAL_MS);
	}

	private executeNextCommand(): void {
		if (!this.isPlaying || this.isPaused || this.config.sequences.length === 0) {
			return;
		}

		const currentSequence = this.config.sequences[this.currentSequenceIndex];
		if (!currentSequence) {
			this.handleSequenceEnd();
			return;
		}

		const currentCommand = currentSequence.commands[this.currentCommandIndex];
		if (!currentCommand) {
			this.handleCommandEnd();
			return;
		}

		console.log(
			`Executing command ${this.currentCommandIndex + 1}/${currentSequence.commands.length} from sequence "${currentSequence.name}"`
		);

		// Send the command
		this.notifyCommand([currentCommand]);

		// Move to next command
		this.currentCommandIndex++;
	}

	private handleCommandEnd(): void {
		const currentSequence = this.config.sequences[this.currentSequenceIndex];

		// Sequence completed, notify
		this.notifySequence(currentSequence);

		// Move to next sequence or loop
		this.currentCommandIndex = 0;
		this.currentSequenceIndex++;

		if (this.currentSequenceIndex >= this.config.sequences.length) {
			if (this.config.loopMode) {
				console.log("Looping back to first sequence");
				this.currentSequenceIndex = 0;
			} else {
				console.log("All sequences completed");
				this.stop();
			}
		}
	}

	private handleSequenceEnd(): void {
		if (this.config.loopMode) {
			this.currentSequenceIndex = 0;
			this.currentCommandIndex = 0;
		} else {
			this.stop();
		}
	}

	private notifyCommand(commands: RobotCommand[]): void {
		this.commandCallbacks.forEach((callback) => {
			try {
				callback(commands);
			} catch (error) {
				console.error("Error in command callback:", error);
			}
		});
	}

	private notifySequence(sequence: CommandSequence): void {
		this.sequenceCallbacks.forEach((callback) => {
			try {
				callback(sequence);
			} catch (error) {
				console.error("Error in sequence callback:", error);
			}
		});
	}

	private notifyStatusChange(): void {
		this.statusCallbacks.forEach((callback) => {
			try {
				callback(this._status);
			} catch (error) {
				console.error("Error in status callback:", error);
			}
		});
	}
}

// Predefined demo sequences
export const DEMO_SEQUENCES: CommandSequence[] = [
	{
		id: "gentle-wave",
		name: "Gentle Wave Pattern",
		totalDuration: 6000,
		commands: [
			{
				timestamp: 0,
				joints: [
					{ name: "Rotation", value: -10 },
					{ name: "Pitch", value: 8 },
					{ name: "Elbow", value: -12 }
				],
				duration: 2000
			},
			{
				timestamp: 2000,
				joints: [{ name: "Wrist_Roll", value: 10 }],
				duration: 1000
			},
			{
				timestamp: 3000,
				joints: [{ name: "Wrist_Roll", value: -10 }],
				duration: 1000
			},
			{
				timestamp: 4000,
				joints: [
					{ name: "Wrist_Roll", value: 0 },
					{ name: "Rotation", value: 0 },
					{ name: "Pitch", value: 0 },
					{ name: "Elbow", value: 0 }
				],
				duration: 2000
			}
		]
	},
	{
		id: "small-scan",
		name: "Small Scanning Pattern",
		totalDuration: 8000,
		commands: [
			{
				timestamp: 0,
				joints: [
					{ name: "Rotation", value: -15 },
					{ name: "Pitch", value: 10 }
				],
				duration: 2000
			},
			{
				timestamp: 2000,
				joints: [{ name: "Rotation", value: 15 }],
				duration: 3000
			},
			{
				timestamp: 5000,
				joints: [
					{ name: "Rotation", value: 0 },
					{ name: "Pitch", value: 0 }
				],
				duration: 3000
			}
		]
	},
	{
		id: "tiny-flex",
		name: "Tiny Flex Pattern",
		totalDuration: 8000,
		commands: [
			{
				timestamp: 0,
				joints: [
					{ name: "Elbow", value: -15 },
					{ name: "Wrist_Pitch", value: 8 }
				],
				duration: 2000
			},
			{
				timestamp: 2000,
				joints: [{ name: "Jaw", value: 8 }],
				duration: 1000
			},
			{
				timestamp: 3000,
				joints: [{ name: "Elbow", value: -25 }],
				duration: 2000
			},
			{
				timestamp: 5000,
				joints: [
					{ name: "Jaw", value: 0 },
					{ name: "Elbow", value: 0 },
					{ name: "Wrist_Pitch", value: 0 }
				],
				duration: 3000
			}
		]
	}
];