using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks;
using HC_APTBS.Models;

namespace HC_APTBS.Services.Impl
{
    /// <summary>
    /// Implements the full bench test orchestration: RPM control, temperature PID,
    /// relay management, and phase-by-phase test execution.
    ///
    /// <para>
    /// Test execution runs in a <see cref="Task"/> (background thread).  All events
    /// are raised on that thread — consumers (ViewModels) must marshal to the UI thread.
    /// </para>
    /// </summary>
    public sealed class BenchService : IBenchService
    {
        // ── Dependencies ──────────────────────────────────────────────────────────

        private readonly ICanService _can;
        private readonly IConfigurationService _config;
        private readonly IAppLogger _log;
        private const string LogId = "BenchService";

        // ── State ─────────────────────────────────────────────────────────────────

        private PumpDefinition? _activePump;
        private CancellationTokenSource? _cts;
        private volatile bool _running;

        // Temperature PID state
        private double _temperatureSetpoint;
        private double _temperatureTolerance;

        // DFI auto-adjust flag (set by the UI)
        private volatile bool _autoDfiEnabled;

        // Periodic pump parameter CAN sender
        private Task? _pumpSendTask;
        private CancellationTokenSource? _pumpSendCts;
        private volatile bool _pumpSendingActive;
        private double _targetMe;
        private double _targetFbkw;
        private double _targetPreIn;

        // Periodic MemoryRequest sender (for Tein acquisition)
        private Task? _memoryRequestTask;
        private CancellationTokenSource? _memoryRequestCts;
        private volatile bool _memoryRequestActive;

        // Periodic ElectronicMsg keepalive sender
        private Task? _electronicMsgTask;
        private CancellationTokenSource? _electronicMsgCts;
        private volatile bool _electronicMsgActive;

        // Periodic relay bitmask sender
        private const int RelaySendIntervalMs = 21;
        private Task? _relaySendTask;
        private CancellationTokenSource? _relaySendCts;
        private volatile bool _relaySendActive;

        // RPM PID ramp controller
        private BenchPidController? _pidController;
        private double _lastTargetRpm;
        private double _lastCommandVoltage;
        private double _pidTargetRpm;

        // ── Events ────────────────────────────────────────────────────────────────

        /// <inheritdoc/>
        public event Action?                            TestStarted;
        /// <inheritdoc/>
        public event Action<bool, bool>?                TestFinished;
        /// <inheritdoc/>
        public event Action<string>?                    PhaseChanged;
        /// <inheritdoc/>
        public event Action<string>?                    VerboseMessage;
        /// <inheritdoc/>
        public event Action?                            PsgSyncError;
        /// <inheritdoc/>
        public event Action?                            LockAngleFaseReady;
        /// <inheritdoc/>
        public event Action?                            PsgModeFaseReady;
        /// <inheritdoc/>
        public event Action<string, double, double>?    ToleranceUpdated;
        /// <inheritdoc/>
        public event Action<string, bool>?              PhaseCompleted;
        /// <inheritdoc/>
        public event Action<string, double>?            PumpControlValueSet;
        /// <inheritdoc/>
        public event Action?                            RpmCommandSent;

        // ── Constructor ───────────────────────────────────────────────────────────

        /// <param name="canService">CAN bus service for parameter I/O.</param>
        /// <param name="configService">Configuration providing bench parameters and app settings.</param>
        /// <param name="logger">Application logger.</param>
        public BenchService(ICanService canService, IConfigurationService configService, IAppLogger logger)
        {
            _can    = canService;
            _config = configService;
            _log    = logger;
        }

        // ── IBenchService: properties ─────────────────────────────────────────────

        /// <inheritdoc/>
        public bool IsAutoDfiEnabled => _autoDfiEnabled;

        /// <inheritdoc/>
        public double LastTargetRpm => _lastTargetRpm;

        /// <inheritdoc/>
        public double LastCommandVoltage => _lastCommandVoltage;

        // ── IBenchService: active pump ────────────────────────────────────────────

        /// <inheritdoc/>
        public void SetActivePump(PumpDefinition? pump)
        {
            _activePump = pump;
            _log.Debug(LogId, $"Active pump set: {pump?.Id ?? "(none)"}");
        }

        // ── IBenchService: parameter access ───────────────────────────────────────

        /// <inheritdoc/>
        public double ReadBenchParameter(string parameterName)
        {
            if (_config.Bench.ParametersByName.TryGetValue(parameterName, out var benchParam))
                return benchParam.Value;
            return 0;
        }

        /// <inheritdoc/>
        public double ReadPumpParameter(string parameterName)
        {
            if (_activePump != null &&
                _activePump.ParametersByName.TryGetValue(parameterName, out var pumpParam))
                return pumpParam.Value;
            return 0;
        }

        /// <inheritdoc/>
        public double ReadParameter(string parameterName)
        {
            if (_activePump != null &&
                _activePump.ParametersByName.TryGetValue(parameterName, out var pumpParam))
                return pumpParam.Value;
            if (_config.Bench.ParametersByName.TryGetValue(parameterName, out var benchParam))
                return benchParam.Value;
            return 0;
        }

        /// <inheritdoc/>
        public void SetParameter(string parameterName, double value)
        {
            if (_config.Bench.ParametersByName.TryGetValue(parameterName, out var benchParam))
                benchParam.Value = value;
            else if (_activePump != null &&
                     _activePump.ParametersByName.TryGetValue(parameterName, out var pumpParam))
                pumpParam.Value = value;
        }

        /// <inheritdoc/>
        public void SendParameters(uint messageId)
            => _can.SendMessageById(messageId);

        // ── IBenchService: RPM control ────────────────────────────────────────────

        /// <inheritdoc/>
        public void SetRpm(double rpm)
        {
            // Clamp to configured safety limit.
            double safeRpm = Math.Min(rpm, _config.Settings.SecurityRpmLimit);

            // Look up the required motor control voltage from the RPM-to-voltage table.
            double voltage = RpmVoltageRelation.VoltageForRpm(
                (int)safeRpm, _config.Settings.Relations);

            SendRpmVoltage(voltage);

            _lastTargetRpm = safeRpm;
            RpmCommandSent?.Invoke();
            _log.Debug(LogId, $"SetRpm({safeRpm}) → voltage={voltage:F3}V");
        }

        /// <inheritdoc/>
        public void StartRpmPid(double targetRpm)
        {
            // Stop any existing PID loop.
            _pidController?.Stop();

            double safeRpm = Math.Min(targetRpm, _config.Settings.SecurityRpmLimit);
            _lastTargetRpm = safeRpm;
            _pidTargetRpm = safeRpm;

            if (safeRpm <= 0)
            {
                SendRpmVoltage(0);
                RpmCommandSent?.Invoke();
                return;
            }

            // Step 1: Send initial voltage from lookup table (open-loop jump).
            double initialVoltage = RpmVoltageRelation.VoltageForRpm(
                (int)safeRpm, _config.Settings.Relations);
            SendRpmVoltage(initialVoltage);
            RpmCommandSent?.Invoke();

            // Step 2: Calculate approach delay based on RPM distance.
            double actualRpm = ReadBenchParameter(BenchParameterNames.BenchRpm);
            int delaySec = (int)(Math.Abs(safeRpm - actualRpm) * 0.004 + 0.7);

            // Step 3: Create PID controller and start after delay.
            var settings = _config.Settings;
            _pidController = new BenchPidController(
                settings.PidP, settings.PidI, settings.PidD, settings.PidLoopMs,
                settings.MaxRpm, settings.VoltageForMaxRpm,
                () => ReadBenchParameter(BenchParameterNames.BenchRpm),
                () => _pidTargetRpm,
                SendRpmVoltage);

            _ = Task.Run(async () =>
            {
                try
                {
                    await Task.Delay(delaySec * 1000);
                    if (_pidTargetRpm > 0)
                        _pidController.Start(initialVoltage, actualRpm);
                }
                catch (Exception ex)
                {
                    _log.Error(LogId, $"StartRpmPid delay task: {ex.Message}");
                }
            });

            _log.Info(LogId, $"StartRpmPid({safeRpm}) → initial voltage={initialVoltage:F3}V, PID delay={delaySec}s");
        }

        /// <inheritdoc/>
        public void StopRpmPid()
        {
            _pidTargetRpm = 0;
            _pidController?.Stop();
            _pidController?.Dispose();
            _pidController = null;
            _lastTargetRpm = 0;
            SendRpmVoltage(0);
            RpmCommandSent?.Invoke();
            _log.Info(LogId, "StopRpmPid: PID stopped, 0V sent.");
        }

        /// <summary>
        /// Writes a voltage value to the RPM CAN parameter and transmits it.
        /// Used as the PID output callback and by <see cref="SetRpm"/>.
        /// </summary>
        private void SendRpmVoltage(double volts)
        {
            if (volts < 0) volts = 0;
            _lastCommandVoltage = volts;

            SetParameter(BenchParameterNames.Rpm, volts);
            SendParameters(_config.Bench.ParametersByName.TryGetValue(
                BenchParameterNames.Rpm, out var rpmParam) ? rpmParam.MessageId : 0x10);
        }

        // ── IBenchService: temperature ────────────────────────────────────────────

        /// <inheritdoc/>
        public bool SetTemperatureSetpoint(double setpointCelsius, double toleranceCelsius)
        {
            _temperatureSetpoint  = setpointCelsius;
            _temperatureTolerance = toleranceCelsius;

            // Return true (ignore temperature) if DefaultIgnoreTin is configured.
            return _config.Settings.DefaultIgnoreTin;
        }

        // ── IBenchService: relay control ──────────────────────────────────────────

        /// <inheritdoc/>
        public void SetRelay(string relayName, bool state)
        {
            if (!_config.Bench.Relays.TryGetValue(relayName, out var relay)) return;

            relay.State = state;
            // Rebuild the relay bitmask and transmit.
            TransmitRelayMask(relay.MessageId);
        }

        /// <inheritdoc/>
        public void SendInitialRelayState()
        {
            // Collect all distinct relay message IDs and transmit each one.
            var sent = new HashSet<uint>();
            foreach (var relay in _config.Bench.Relays.Values)
            {
                if (sent.Add(relay.MessageId))
                    TransmitRelayMask(relay.MessageId);
            }
        }

        /// <inheritdoc/>
        public void StartRelaySender()
        {
            if (_relaySendActive) return;
            _relaySendActive = true;

            _relaySendCts = new CancellationTokenSource();
            var ct = _relaySendCts.Token;

            _relaySendTask = Task.Run(async () =>
            {
                using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(RelaySendIntervalMs));
                try
                {
                    while (_relaySendActive && await timer.WaitForNextTickAsync(ct))
                    {
                        var sent = new HashSet<uint>();
                        foreach (var relay in _config.Bench.Relays.Values)
                        {
                            if (sent.Add(relay.MessageId))
                                TransmitRelayMask(relay.MessageId);
                        }
                    }
                }
                catch (OperationCanceledException) { }
            }, ct);

            _log.Debug(LogId, "Relay sender started.");
        }

        /// <inheritdoc/>
        public void StopRelaySender()
        {
            _relaySendActive = false;
            _relaySendCts?.Cancel();
            _relaySendCts?.Dispose();
            _relaySendCts = null;
            _log.Debug(LogId, "Relay sender stopped.");
        }

        private void TransmitRelayMask(uint messageId)
        {
            // Collect all relays mapped to this CAN message ID and pack their bits.
            long mask = 0;
            foreach (var relay in _config.Bench.Relays.Values)
            {
                if (relay.MessageId == messageId && relay.State)
                    mask |= (1L << relay.Bit);
            }

            byte[] data = new byte[8];
            for (int i = 0; i < 8; i++)
                data[i] = (byte)((mask >> (i * 8)) & 0xFF);

            _can.SendRawMessage(messageId, data);
        }

        // ── IBenchService: pump control sender ─────────────────────────────────────

        /// <inheritdoc/>
        public void SetPumpControlValue(string parameterName, double targetValue)
        {
            if (_activePump == null) return;

            switch (parameterName)
            {
                case PumpParameterNames.Me:
                    _targetMe = targetValue;
                    break;
                case PumpParameterNames.Fbkw:
                    _targetFbkw = targetValue;
                    // FBKW is written directly with no slew-rate filtering.
                    if (_activePump.ParametersByName.TryGetValue(PumpParameterNames.Fbkw, out var fbkwParam))
                        fbkwParam.Value = targetValue;
                    break;
                case PumpParameterNames.PreIn:
                    _targetPreIn = targetValue;
                    break;
            }

            StartPumpSender();
        }

        /// <inheritdoc/>
        public void StartPumpSender()
        {
            if (_pumpSendingActive) return;
            _pumpSendingActive = true;

            _pumpSendCts = new CancellationTokenSource();
            var ct = _pumpSendCts.Token;
            int intervalMs = Math.Max(1, _config.Settings.RefreshPumpParamsMs);

            _pumpSendTask = Task.Run(async () =>
            {
                using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(intervalMs));
                try
                {
                    while (_pumpSendingActive && await timer.WaitForNextTickAsync(ct))
                    {
                        PumpSendTick();
                    }
                }
                catch (OperationCanceledException) { }
            }, ct);
        }

        /// <inheritdoc/>
        public void StopPumpSender()
        {
            _pumpSendingActive = false;
            _pumpSendCts?.Cancel();
            _pumpSendCts?.Dispose();
            _pumpSendCts = null;
        }

        private void PumpSendTick()
        {
            if (_activePump == null) return;

            // ME: apply slew-rate IIR filter (alpha * 0.2) before sending.
            CanBusParameter? meParam = null;
            if (_activePump.ParametersByName.TryGetValue(PumpParameterNames.Me, out meParam))
            {
                double alpha = meParam.Alpha * 0.2;
                meParam.Value = Math.Round(meParam.Value + alpha * (_targetMe - meParam.Value), 4);
                _can.SendMessageById(meParam.MessageId);
            }

            // FBKW: send its message if it uses a different CAN ID than ME.
            if (_activePump.ParametersByName.TryGetValue(PumpParameterNames.Fbkw, out var fbkwParam))
            {
                if (meParam == null || fbkwParam.MessageId != meParam.MessageId)
                    _can.SendMessageById(fbkwParam.MessageId);
            }

            // PreIn: apply slew-rate IIR filter (full alpha) before sending.
            if (_activePump.HasPreInjection &&
                _activePump.ParametersByName.TryGetValue(PumpParameterNames.PreIn, out var preinParam))
            {
                preinParam.Value = Math.Round(
                    preinParam.Value + preinParam.Alpha * (_targetPreIn - preinParam.Value), 4);
                _can.SendMessageById(preinParam.MessageId);
            }
        }

        // ── IBenchService: MemoryRequest sender ────────────────────────────────────

        /// <inheritdoc/>
        public void StartMemoryRequestSender()
        {
            if (_memoryRequestActive) return;
            if (_activePump == null ||
                !_activePump.ParametersByName.ContainsKey(PumpParameterNames.MemoryRequest))
                return;

            _memoryRequestActive = true;
            _memoryRequestCts = new CancellationTokenSource();
            var ct = _memoryRequestCts.Token;
            int intervalMs = Math.Max(1, _config.Settings.RefreshPumpRequestMs);

            _memoryRequestTask = Task.Run(async () =>
            {
                using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(intervalMs));
                try
                {
                    while (_memoryRequestActive && await timer.WaitForNextTickAsync(ct))
                    {
                        if (_activePump?.ParametersByName.TryGetValue(
                                PumpParameterNames.MemoryRequest, out var memReqParam) == true)
                        {
                            _can.SendMessageById(memReqParam.MessageId);
                        }
                    }
                }
                catch (OperationCanceledException) { }
            }, ct);

            _log.Debug(LogId, "MemoryRequest sender started.");
        }

        /// <inheritdoc/>
        public void StopMemoryRequestSender()
        {
            _memoryRequestActive = false;
            _memoryRequestCts?.Cancel();
            _memoryRequestCts?.Dispose();
            _memoryRequestCts = null;
            _log.Debug(LogId, "MemoryRequest sender stopped.");
        }

        // ── IBenchService: ElectronicMsg keepalive sender ─────────────────────────

        /// <inheritdoc/>
        public void StartElectronicMsgSender()
        {
            if (_electronicMsgActive) return;
            _electronicMsgActive = true;

            _electronicMsgCts = new CancellationTokenSource();
            var ct = _electronicMsgCts.Token;

            _electronicMsgTask = Task.Run(async () =>
            {
                using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(1000));
                try
                {
                    while (_electronicMsgActive && await timer.WaitForNextTickAsync(ct))
                    {
                        ElectronicMsgTick();
                    }
                }
                catch (OperationCanceledException) { }
            }, ct);

            _log.Debug(LogId, "ElectronicMsg keepalive sender started.");
        }

        /// <inheritdoc/>
        public void StopElectronicMsgSender()
        {
            _electronicMsgActive = false;
            _electronicMsgCts?.Cancel();
            _electronicMsgCts?.Dispose();
            _electronicMsgCts = null;
            _log.Debug(LogId, "ElectronicMsg keepalive sender stopped.");
        }

        private void ElectronicMsgTick()
        {
            if (!_config.Bench.ParametersByName.TryGetValue(
                    BenchParameterNames.ElectronicMsg, out var electronicParam))
                return;

            byte[] data = new byte[8];
            data[0] = 1;

            // Encode the encoder resolution into its assigned byte positions.
            if (_config.Bench.ParametersByName.TryGetValue(
                    BenchParameterNames.EncoderResolution, out var encoderParam))
            {
                int resolution = (int)_config.Settings.EncoderResolution;
                data[encoderParam.ByteH] = (byte)((resolution & 0xFF00) >> 8);
                data[encoderParam.ByteL] = (byte)(resolution & 0x00FF);
            }

            _can.SendRawMessage(electronicParam.MessageId, data);
        }

        // ── IBenchService: DFI ────────────────────────────────────────────────────

        /// <inheritdoc/>
        public void SetDfi(double dfi)
        {
            _log.Debug(LogId, $"SetDfi({dfi:F4}) — forwarded to KWP service via event.");
            // The ViewModel wires this to IKwpService.WriteDfiAsync in the test flow.
        }

        // ── IBenchService: test execution ─────────────────────────────────────────

        /// <inheritdoc/>
        public async Task RunTestsAsync(PumpDefinition pump, CancellationToken ct)
        {
            _running = true;
            _cts     = CancellationTokenSource.CreateLinkedTokenSource(ct);

            try
            {
                TestStarted?.Invoke();

                // Small delay to allow oil circulation to stabilise before the first test.
                await Task.Delay(2000, _cts.Token);

                bool overallSuccess = true;
                TestDefinition? lastTest = null;

                foreach (var test in pump.Tests)
                {
                    if (_cts.Token.IsCancellationRequested) break;
                    if (!test.HasActivePhase()) continue;

                    lastTest = test;
                    bool testSuccess = await RunSingleTestAsync(test, pump, _cts.Token);
                    overallSuccess = overallSuccess && testSuccess;

                    if (!testSuccess && ShouldHaltOnFailure(test)) break;
                }

                bool wasInterrupted = _cts.Token.IsCancellationRequested;
                TestFinished?.Invoke(wasInterrupted, wasInterrupted ? false : overallSuccess);
            }
            catch (OperationCanceledException)
            {
                TestFinished?.Invoke(true, false);
            }
            catch (Exception ex)
            {
                _log.Error(LogId, $"RunTestsAsync exception: {ex}");
                TestFinished?.Invoke(true, false);
            }
            finally
            {
                _running = false;
            }
        }

        /// <inheritdoc/>
        public void StopTests()
        {
            _cts?.Cancel();
            SetRpm(0);
            _log.Info(LogId, "Test sequence stopped by operator.");
        }

        // ── Private: test execution ───────────────────────────────────────────────

        private async Task<bool> RunSingleTestAsync(
            TestDefinition test, PumpDefinition pump, CancellationToken ct)
        {
            bool success = true;

            foreach (var phase in test.Phases)
            {
                if (!phase.Enabled || ct.IsCancellationRequested) break;

                PhaseChanged?.Invoke(phase.Name);
                phase.Success = true;
                phase.ClearResults();
                phase.ErrorBits.Clear();

                // SVME test: check that the PSG encoder sync pulse is present before proceeding.
                if (!phase.Name.Contains("Lock Angle") &&
                    test.Name == TestType.Svme &&
                    ReadBenchParameter(BenchParameterNames.PsgEncoderWorking) == 0)
                {
                    VerboseMessage?.Invoke($"{phase.Name} — PSG Sync Pulse Error");
                    PsgSyncError?.Invoke();
                    return false;
                }

                // ── Step 1: Set RPM and wait for bench to reach setpoint ──────────
                var rpmSetpoint = phase.GetRpmSetpoint();
                if (rpmSetpoint != null)
                {
                    SetRpm(rpmSetpoint.Value);
                    VerboseMessage?.Invoke($"{phase.Name} — Revving to {rpmSetpoint.Value} RPM...");

                    // Wait on the bench motor encoder feedback (BenchRPM), not the
                    // ambiguous "RPM" which resolves to pump ECU RPM when a pump is loaded.
                    await WaitForParameter(
                        BenchParameterNames.BenchRpm, rpmSetpoint.Value, rpmSetpoint.Tolerance, ct);

                    // 2-second stabilisation delay after reaching target RPM.
                    await Task.Delay(2000, ct);
                }

                ct.ThrowIfCancellationRequested();
                var sw = Stopwatch.StartNew();

                // ── Step 2: Send pump parameters ──────────────────────────────────
                VerboseMessage?.Invoke($"{phase.Name} — Sending pump parameters...");
                foreach (var send in phase.Sends)
                {
                    ct.ThrowIfCancellationRequested();
                    if (send.Name == BenchParameterNames.Rpm) continue; // already handled

                    // Pump control params route through the periodic sender with slew-rate filtering.
                    if (send.Name == PumpParameterNames.Me ||
                        send.Name == PumpParameterNames.Fbkw ||
                        send.Name == PumpParameterNames.PreIn)
                    {
                        SetPumpControlValue(send.Name, send.Value);
                        PumpControlValueSet?.Invoke(send.Name, send.Value);
                    }
                    else
                    {
                        SetParameter(send.Name, send.Value);
                    }
                }
                // Transmit bench parameters that were set above.
                foreach (var send in phase.Sends)
                {
                    if (send.Name == BenchParameterNames.Rpm ||
                        send.Name == PumpParameterNames.Me ||
                        send.Name == PumpParameterNames.Fbkw ||
                        send.Name == PumpParameterNames.PreIn) continue;

                    if (_config.Bench.ParametersByName.TryGetValue(send.Name, out var sp))
                    {
                        SendParameters(sp.MessageId);
                        break; // one frame per message ID group is enough
                    }
                }

                // ── Step 3: Wait for temperature setpoint ─────────────────────────
                if (phase.Readies.Count > 0)
                {
                    var tempReady = phase.Readies[0];
                    bool ignoreTin = SetTemperatureSetpoint(tempReady.Value, tempReady.Tolerance);
                    if (!ignoreTin)
                    {
                        VerboseMessage?.Invoke($"{phase.Name} — Adjusting temperature...");
                        await WaitForParameter(
                            tempReady.Name, tempReady.Value, tempReady.Tolerance, ct);
                    }
                }

                // Notify chart view of expected tolerance bands.
                foreach (var recv in phase.Receives)
                {
                    if (recv.Name == BenchParameterNames.QDelivery ||
                        recv.Name == BenchParameterNames.QOver)
                        ToleranceUpdated?.Invoke(recv.Name, recv.Value, recv.Tolerance);
                }

                // ── Step 4: Conditioning time countdown ───────────────────────────
                sw.Stop();
                long conditioningRemainMs = (long)test.ConditioningTimeSec * 1000 - sw.ElapsedMilliseconds;
                _log.Debug(LogId, $"{phase.Name}: conditioning remaining={conditioningRemainMs}ms");

                for (int i = 0; i * 1000 < conditioningRemainMs; i++)
                {
                    ct.ThrowIfCancellationRequested();
                    int remaining = (int)(conditioningRemainMs / 1000) - i;
                    VerboseMessage?.Invoke($"{phase.Name} — Conditioning... {remaining}s");
                    await Task.Delay(1000, ct);
                }

                ct.ThrowIfCancellationRequested();

                // ── Step 5: Special phase handling ────────────────────────────────
                if (phase.Name.Contains("Lock Angle"))
                {
                    VerboseMessage?.Invoke($"{phase.Name} — Acquiring Lock Angle...");
                    LockAngleFaseReady?.Invoke();
                    await Task.Delay(200, ct);
                    continue; // No measurement collection for lock angle phases
                }

                if (phase.Name.Contains(TestDefinition.XmlPhase) || phase.Name.Contains("PSG(0)"))
                {
                    VerboseMessage?.Invoke($"{phase.Name} — PSG Mode...");
                    PsgModeFaseReady?.Invoke();
                    await Task.Delay(4000, ct);
                    continue;
                }

                // ── Step 6: DFI auto-adjust loop ──────────────────────────────────
                if (test.Name == TestType.Dfi && _autoDfiEnabled)
                {
                    phase.Success = await RunDfiAutoAdjustAsync(test, phase, ct);
                }
                else
                {
                    phase.Success = await MeasurePhaseAsync(test, phase, ct);
                }

                success = success && phase.Success;
                PhaseCompleted?.Invoke(phase.Name, phase.Success);

                // Critical phase failure halts the entire test.
                if (phase.IsCritical && !phase.Success)
                {
                    SetRpm(0);
                    VerboseMessage?.Invoke($"CRITICAL failure in {phase.Name} — test halted.");
                    return false;
                }

                // Stop pump between phases (motor cool-down).
                SetRpm(0);
            }

            return success;
        }

        /// <summary>
        /// Collects measurements for <paramref name="phase"/> over the measurement window,
        /// then evaluates pass/fail for each receive parameter.
        /// </summary>
        private async Task<bool> MeasurePhaseAsync(
            TestDefinition test, PhaseDefinition phase, CancellationToken ct)
        {
            VerboseMessage?.Invoke($"{phase.Name} — Collecting measurements...");

            // Clear previous results.
            foreach (var tp in phase.Receives)
                tp.Result = new TestResult { TestType = test.Name };

            long measureMs = (long)test.MeasurementTimeSec * 1000;
            int sleepMs    = (int)(1000.0 / Math.Max(test.MeasurementsPerSecond, 0.1));
            var sw         = Stopwatch.StartNew();

            while (sw.ElapsedMilliseconds <= measureMs)
            {
                ct.ThrowIfCancellationRequested();

                foreach (var tp in phase.Receives)
                {
                    var sample = new MeasurementSample
                    {
                        Value     = ReadParameter(tp.Name),
                        Timestamp = DateTime.Now.ToString(TestDefinition.TimestampFormat)
                    };
                    tp.Result!.AddSample(sample);
                }

                await Task.Delay(sleepMs, ct);
            }

            // Evaluate pass/fail.
            bool phaseSuccess = true;
            foreach (var tp in phase.Receives)
            {
                tp.Result!.Evaluate(
                    tp.Value, tp.Tolerance,
                    _config.Settings.ToleranceUpExtension,
                    _config.Settings.TolerancePfpExtension);
                phaseSuccess = phaseSuccess && tp.Result.Passed;
            }

            return phaseSuccess;
        }

        /// <summary>
        /// Auto-adjusts the DFI calibration angle iteratively until the measured delivery
        /// matches the target, or the test is cancelled.
        ///
        /// <para>
        /// The relationship between DFI angle and delivery quantity was determined
        /// experimentally: <c>delivery = 15.5 × DFI + 32.3</c>.
        /// The inverse gives the required DFI to achieve a target delivery.
        /// </para>
        /// </summary>
        private async Task<bool> RunDfiAutoAdjustAsync(
            TestDefinition test, PhaseDefinition phase, CancellationToken ct)
        {
            // Baseline measurement.
            await MeasurePhaseAsync(test, phase, ct);
            if (phase.Receives.Count == 0) return false;

            var target = phase.Receives[0];
            if (target.Result == null || Math.Abs(target.Result.Average - target.Value) <= target.Tolerance)
                return target.Result?.Passed ?? false;

            double initialDfi    = ReadParameter(KlineKeys.Dfi);
            // Empirically derived linear model: delivery = 15.5 × DFI + 32.3
            double displacement  = 15.5 * initialDfi + 32.3 - target.Result.Average;
            double lastError     = 0;

            while (!ct.IsCancellationRequested)
            {
                if (Math.Abs(target.Result!.Average - target.Value) <= target.Tolerance)
                    break;

                double sendDfi = (target.Value + displacement - 32.3 + lastError) / 15.5;
                sendDfi = Math.Clamp(sendDfi, -1.45, 1.45); // Hardware DFI range ±1.45°

                VerboseMessage?.Invoke($"{phase.Name} — Writing DFI: {sendDfi:F4}°");
                SetDfi(sendDfi);

                // Re-conditioning period before re-measuring.
                for (int i = 0; i < test.ConditioningTimeSec; i++)
                {
                    ct.ThrowIfCancellationRequested();
                    VerboseMessage?.Invoke($"{phase.Name} — Conditioning... {test.ConditioningTimeSec - i}s");
                    await Task.Delay(2000, ct); // 2 s steps — gives ECU time to reinitialise K-Line
                }

                await MeasurePhaseAsync(test, phase, ct);
                lastError = target.Value - target.Result.Average;
            }

            return target.Result?.Passed ?? false;
        }

        // ── Helpers ───────────────────────────────────────────────────────────────

        private async Task WaitForParameter(
            string name, double target, double tolerance, CancellationToken ct)
        {
            while (!ct.IsCancellationRequested)
            {
                double current = ReadParameter(name);
                if (Math.Abs(current - target) <= tolerance) return;
                await Task.Delay(5, ct);
            }
        }

        private static bool ShouldHaltOnFailure(TestDefinition test)
        {
            // Currently no test-level "critical" flag; only phase-level.
            // Preserve this as an extension point.
            return false;
        }
    }
}