import {
    DEFAULT_PREDICTION_OPTIONS,
    type HistoryPoint,
    type PredictionOptions,
    type Rotation,
} from "../types.js";

export class RotationPredictor {
    private history: HistoryPoint<Rotation>[] = [];
    private options: PredictionOptions;
    private lastPrediction: Rotation | null = null;
    private lastPredictionError = 0;

    constructor(options: PredictionOptions = {}) {
        this.options = { ...DEFAULT_PREDICTION_OPTIONS, ...options };
    }

    addRotation(rotation: Rotation, timestamp: number): void {
        this.history.push({ value: { ...rotation }, timestamp });
        const historySize =
            this.options.historySize || DEFAULT_PREDICTION_OPTIONS.historySize;
        if (this.history.length > historySize) {
            this.history.shift();
        }
        if (this.lastPrediction && this.history.length >= 2) {
            this.lastPredictionError = Math.sqrt(
                this.angleDifference(this.lastPrediction.yaw, rotation.yaw) **
                    2 +
                    this.angleDifference(
                        this.lastPrediction.pitch,
                        rotation.pitch
                    ) **
                        2
            );
        }
    }

    private angleDifference(a: number, b: number): number {
        let diff = ((b - a + 180) % 360) - 180;
        if (diff < -180) diff += 360;
        return diff;
    }

    predictRotation(deltaTime: number): Rotation | null {
        if (this.history.length < 2) return null;
        const maxTime =
            this.options.maxPredictionTime ||
            DEFAULT_PREDICTION_OPTIONS.maxPredictionTime;
        const limitedDeltaTime = Math.min(deltaTime, maxTime);
        const dt = limitedDeltaTime / 1000;
        const prediction = this.linearPredictRotation(dt);
        this.lastPrediction = { ...prediction };
        return prediction;
    }

    private linearPredictRotation(dt: number): Rotation {
        const lastItem = this.history[this.history.length - 1];
        if (!lastItem)
            throw new Error("History item required for rotation prediction");
        const last = lastItem.value;
        const angularVelocity = this.calculateAngularVelocity();
        return {
            yaw: this.normalizeAngle(last.yaw + angularVelocity.yaw * dt),
            pitch: this.clampPitch(last.pitch + angularVelocity.pitch * dt),
        };
    }

    private calculateAngularVelocity(): Rotation {
        if (this.history.length < 2) return { yaw: 0, pitch: 0 };
        const last = this.history[this.history.length - 1];
        const prev = this.history[this.history.length - 2];
        if (!last || !prev) return { yaw: 0, pitch: 0 };
        const dt = (last.timestamp - prev.timestamp) / 1000;
        if (dt <= 0) return { yaw: 0, pitch: 0 };
        return {
            yaw: this.angleDifference(prev.value.yaw, last.value.yaw) / dt,
            pitch:
                this.angleDifference(prev.value.pitch, last.value.pitch) / dt,
        };
    }

    private normalizeAngle(angle: number): number {
        return ((angle + 180) % 360) - 180;
    }

    private clampPitch(pitch: number): number {
        return Math.max(-90, Math.min(90, pitch));
    }

    getPredictionError(): number {
        return this.lastPredictionError;
    }

    clearHistory(): void {
        this.history = [];
        this.lastPrediction = null;
        this.lastPredictionError = 0;
    }

    setOptions(options: PredictionOptions): void {
        this.options = { ...this.options, ...options };
    }
}