src/lib/elements/robot/calibration/CalibrationState.svelte.ts

import type { JointCalibration } from "../models.js";
import { ROBOT_CONFIG } from "../config.js";

export class CalibrationState {
	// Reactive calibration state
	isCalibrating = $state(false);
	progress = $state(0);

	// Joint calibration data
	private jointCalibrations = $state<Record<string, JointCalibration>>({});
	private currentValues = $state<Record<string, number>>({});

	// Callbacks for completion with final positions
	private completionCallbacks: Array<(positions: Record<string, number>) => void> = [];

	constructor() {
		// Initialize calibration data for expected joints
		const jointNames = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
		jointNames.forEach((name) => {
			this.jointCalibrations[name] = {
				isCalibrated: false,
				minServoValue: undefined,
				maxServoValue: undefined
			};
			this.currentValues[name] = 0;
		});
	}

	// Computed properties
	get needsCalibration(): boolean {
		return Object.values(this.jointCalibrations).some((cal) => !cal.isCalibrated);
	}

	get isCalibrated(): boolean {
		return Object.values(this.jointCalibrations).every((cal) => cal.isCalibrated);
	}

	// Update current servo value during calibration
	updateCurrentValue(jointName: string, servoValue: number): void {
		this.currentValues[jointName] = servoValue;

		// Update calibration range if calibrating
		if (this.isCalibrating) {
			const calibration = this.jointCalibrations[jointName];
			if (calibration) {
				// Update min/max values
				if (calibration.minServoValue === undefined || servoValue < calibration.minServoValue) {
					calibration.minServoValue = servoValue;
				}
				if (calibration.maxServoValue === undefined || servoValue > calibration.maxServoValue) {
					calibration.maxServoValue = servoValue;
				}

				// Update progress based on range coverage
				this.updateProgress();
			}
		}
	}

	// Get current value for a joint
	getCurrentValue(jointName: string): number | undefined {
		return this.currentValues[jointName];
	}

	// Get calibration data for a joint
	getJointCalibration(jointName: string): JointCalibration | undefined {
		return this.jointCalibrations[jointName];
	}

	// Get formatted range string for display
	getJointRange(jointName: string): string {
		const calibration = this.jointCalibrations[jointName];
		if (
			!calibration ||
			calibration.minServoValue === undefined ||
			calibration.maxServoValue === undefined
		) {
			return "Not set";
		}
		return `${calibration.minServoValue}-${calibration.maxServoValue}`;
	}

	// Format servo value for display
	formatServoValue(value: number | undefined): string {
		return value !== undefined ? value.toString() : "---";
	}

	// Start calibration process
	startCalibration(): void {
		console.log("[CalibrationState] Starting calibration...");
		this.isCalibrating = true;
		this.progress = 0;

		// Reset calibration data
		Object.keys(this.jointCalibrations).forEach((jointName) => {
			this.jointCalibrations[jointName] = {
				isCalibrated: false,
				minServoValue: undefined,
				maxServoValue: undefined
			};
		});
	}

	// Complete calibration and mark joints as calibrated
	completeCalibration(): Record<string, number> {
		console.log("[CalibrationState] Completing calibration...");

		const finalPositions: Record<string, number> = {};

		// Mark all joints with sufficient range as calibrated
		Object.keys(this.jointCalibrations).forEach((jointName) => {
			const calibration = this.jointCalibrations[jointName];
			if (calibration.minServoValue !== undefined && calibration.maxServoValue !== undefined) {
				const range = calibration.maxServoValue - calibration.minServoValue;
				if (range >= ROBOT_CONFIG.calibration.minRangeThreshold) {
					calibration.isCalibrated = true;
					finalPositions[jointName] = this.currentValues[jointName] || 0;
					console.log(
						`[CalibrationState] Joint ${jointName} calibrated: ${this.getJointRange(jointName)} (range: ${range})`
					);
				} else {
					console.warn(
						`[CalibrationState] Joint ${jointName} range too small: ${range} < ${ROBOT_CONFIG.calibration.minRangeThreshold}`
					);
				}
			}
		});

		this.isCalibrating = false;
		this.progress = 100;

		// Notify completion callbacks
		this.completionCallbacks.forEach((callback) => {
			try {
				callback(finalPositions);
			} catch (error) {
				console.error("[CalibrationState] Error in completion callback:", error);
			}
		});

		return finalPositions;
	}

	// Cancel calibration
	cancelCalibration(): void {
		console.log("[CalibrationState] Calibration cancelled");
		this.isCalibrating = false;
		this.progress = 0;

		// Reset calibration data
		Object.keys(this.jointCalibrations).forEach((jointName) => {
			this.jointCalibrations[jointName] = {
				isCalibrated: false,
				minServoValue: undefined,
				maxServoValue: undefined
			};
		});
	}

	// Skip calibration (use predefined values)
	skipCalibration(): void {
		console.log("[CalibrationState] Skipping calibration with predefined values");

		// Set predefined calibration values for SO-100 arm
		const predefinedCalibrations = {
			Rotation: { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
			Pitch: { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
			Elbow: { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
			Wrist_Pitch: { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
			Wrist_Roll: { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
			Jaw: { minServoValue: 1000, maxServoValue: 3000, isCalibrated: true }
		};

		Object.entries(predefinedCalibrations).forEach(([jointName, calibration]) => {
			this.jointCalibrations[jointName] = calibration;
		});

		this.isCalibrating = false;
		this.progress = 100;
	}

	// Convert raw servo value to normalized percentage (for USB INPUT - reading from servo)
	normalizeValue(rawValue: number, jointName: string): number {
		const calibration = this.jointCalibrations[jointName];
		if (
			!calibration ||
			!calibration.isCalibrated ||
			calibration.minServoValue === undefined ||
			calibration.maxServoValue === undefined
		) {
			// No calibration, use appropriate default conversion
			const isGripper = jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper";
			if (isGripper) {
				return Math.max(0, Math.min(100, (rawValue / 4095) * 100));
			} else {
				return Math.max(-100, Math.min(100, ((rawValue - 2048) / 2048) * 100));
			}
		}

		const { minServoValue, maxServoValue } = calibration;
		if (maxServoValue === minServoValue) return 0;

		// Bound the input servo value to calibrated range
		const bounded = Math.max(minServoValue, Math.min(maxServoValue, rawValue));

		const isGripper = jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper";
		if (isGripper) {
			// Gripper: 0-100%
			return ((bounded - minServoValue) / (maxServoValue - minServoValue)) * 100;
		} else {
			// Regular joint: -100 to +100%
			return ((bounded - minServoValue) / (maxServoValue - minServoValue)) * 200 - 100;
		}
	}

	// Convert normalized percentage to raw servo value (for USB OUTPUT - writing to servo)
	denormalizeValue(normalizedValue: number, jointName: string): number {
		const calibration = this.jointCalibrations[jointName];
		if (
			!calibration ||
			!calibration.isCalibrated ||
			calibration.minServoValue === undefined ||
			calibration.maxServoValue === undefined
		) {
			// No calibration, use appropriate default conversion
			const isGripper = jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper";
			if (isGripper) {
				return Math.round((normalizedValue / 100) * 4095);
			} else {
				return Math.round(2048 + (normalizedValue / 100) * 2048);
			}
		}

		const { minServoValue, maxServoValue } = calibration;
		const range = maxServoValue - minServoValue;

		let normalizedRatio: number;
		const isGripper = jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper";
		if (isGripper) {
			// Gripper: 0-100% -> 0-1
			normalizedRatio = Math.max(0, Math.min(1, normalizedValue / 100));
		} else {
			// Regular joint: -100 to +100% -> 0-1
			normalizedRatio = Math.max(0, Math.min(1, (normalizedValue + 100) / 200));
		}

		return Math.round(minServoValue + normalizedRatio * range);
	}

	// Register callback for calibration completion with final positions
	onCalibrationCompleteWithPositions(
		callback: (positions: Record<string, number>) => void
	): () => void {
		this.completionCallbacks.push(callback);

		// Return unsubscribe function
		return () => {
			const index = this.completionCallbacks.indexOf(callback);
			if (index >= 0) {
				this.completionCallbacks.splice(index, 1);
			}
		};
	}

	// Update progress based on calibration coverage
	private updateProgress(): void {
		if (!this.isCalibrating) return;

		let totalProgress = 0;
		let jointCount = 0;

		Object.values(this.jointCalibrations).forEach((calibration) => {
			jointCount++;
			if (calibration.minServoValue !== undefined && calibration.maxServoValue !== undefined) {
				const range = calibration.maxServoValue - calibration.minServoValue;
				// Progress is based on range size (more range = more progress)
				const jointProgress = Math.min(
					100,
					(range / ROBOT_CONFIG.calibration.minRangeThreshold) * 100
				);
				totalProgress += jointProgress;
			}
		});

		this.progress = jointCount > 0 ? totalProgress / jointCount : 0;
	}

	// Cleanup
	destroy(): void {
		this.completionCallbacks = [];
	}
}