"use strict";
Object.defineProperty(exports, "__esModule", { value: true });
exports.LimitedRangeSystemIdentification = void 0;
const tslib_1 = require("tslib");
const rxjs_1 = require("rxjs");
const os_command_1 = require("./os-command");
const cia402_1 = require("./cia402");
const util_1 = require("./util");
const device_1 = require("./device");
/**
 * Formats a number as a hexadecimal string, prefixed with `0x`.
 *
 * @param value - The numeric value to format.
 * @returns The hexadecimal string representation of the value.
 */
function hex(value) {
    return `0x${value === null || value === void 0 ? void 0 : value.toString(16).toUpperCase()}`;
}
/**
 * Performs a limited-range system identification procedure on a device.
 *
 * This class safely executes the system identification process while preserving
 * the device's original configuration (PID parameters, encoder settings, position
 * feedback filter). It emits notifications about progress and status updates, which
 * can be displayed in a UI or logged for debugging.
 */
class LimitedRangeSystemIdentification {
    /**
     * Creates a new instance of the limited-range system identification routine.
     *
     * @param client - The Motion Master client used for communication with the device.
     * @param deviceRef - A reference to the target device.
     * @param options - Configuration options for the procedure.
     */
    constructor(client, deviceRef, options = {
        hrdStreamingDuration: 3200,
        modesOfOperation: cia402_1.ModesOfOperation.CYCLIC_SYNC_POSITION_MODE,
        rangeLimit: 10000,
        rangeLimitMin: 1000,
        signalType: os_command_1.SystemIdentificationOsCommandSignalType.LINEAR_WITH_CONSTANT_AMPLITUDE,
        startFrequency: 1000,
        startProcedure: os_command_1.SystemIdentificationOsCommandStartProcedure.WAIT_FOR_HRD_STREAMING_TO_START,
        targetAmplitude: 300,
        targetFrequency: 100000,
        transitionTime: 3000,
    }) {
        this.client = client;
        this.deviceRef = deviceRef;
        this.options = options;
        /**
         * Emits status and progress messages during the procedure.
         * Subscribers can display these messages in a UI or log them.
         */
        this.notifications$ = new rxjs_1.Subject();
        /**
         * Indicates whether the system identification procedure is currently running.
         */
        this.isRunning = false;
    }
    /**
     * Starts the limited-range system identification procedure.
     *
     * This method:
     * - Ensures the device is in a safe state.
     * - Computes and sets initial PID values.
     * - Starts data monitoring for position feedback.
     * - Runs the chirp signal on the device.
     * - Computes the plant model without the chirp signal.
     * - Restores all device settings to their original values.
     *
     * Emits notifications to `notifications$` for each major step and error.
     *
     * @throws Will throw an error if any of the device requests fail.
     */
    start() {
        var _a, _b, _c;
        return tslib_1.__awaiter(this, void 0, void 0, function* () {
            // Small delay to ensure subscribers to notifications$ are ready
            yield (0, util_1.resolveAfter)(1);
            // Prevent concurrent executions of the procedure
            if (this.isRunning) {
                this.notifications$.next('Limited Range System Identification is already running.');
                return;
            }
            // Mark the procedure as running
            this.isRunning = true;
            // Validate and adjust HRD streaming duration if necessary
            if (this.options.hrdStreamingDuration < this.options.transitionTime) {
                this.notifications$.next(`Warning: hrdStreamingDuration (${this.options.hrdStreamingDuration} ms) is shorter than transitionTime (${this.options.transitionTime} ms). Increasing hrdStreamingDuration by 1 second to ensure proper timing.`);
                this.options.hrdStreamingDuration = this.options.transitionTime + 1000; // 1-second buffer
            }
            // Ensure the required Modes of Operation is set
            if (this.options.modesOfOperation !== cia402_1.ModesOfOperation.CYCLIC_SYNC_POSITION_MODE) {
                this.notifications$.next(`Warning: modesOfOperation is set to ${this.options.modesOfOperation}, but CYCLIC_SYNC_POSITION_MODE (8) is required for this procedure. Applying the required mode automatically.`);
                this.options.modesOfOperation = cia402_1.ModesOfOperation.CYCLIC_SYNC_POSITION_MODE;
            }
            // Ensure the required Start Procedure is set
            if (this.options.startProcedure !== os_command_1.SystemIdentificationOsCommandStartProcedure.WAIT_FOR_HRD_STREAMING_TO_START) {
                this.notifications$.next(`Warning: startProcedure is currently ${this.options.startProcedure}, but WAIT_FOR_HRD_STREAMING_TO_START (2) is required. Setting the correct start procedure automatically.`);
                this.options.startProcedure = os_command_1.SystemIdentificationOsCommandStartProcedure.WAIT_FOR_HRD_STREAMING_TO_START;
            }
            const gearRatio = yield (0, rxjs_1.lastValueFrom)(this.client.request.computeGearRatio(this.deviceRef, true));
            const rangeLimitEff = this.options.rangeLimit * gearRatio;
            this.notifications$.next(`Effective range limit (considering gear ratio ${gearRatio}): ${rangeLimitEff}`);
            // Variables to hold original configurations for restoration later
            let encCom;
            let encPos;
            const dataMonitoring = this.client.createDataMonitoring([
                [this.deviceRef, 0x20f0, 0x00],
                [this.deviceRef, 0x6041, 0x00],
                [this.deviceRef, 0x6064, 0x00], // Position actual value
            ], 1000 // 1 ms interval
            );
            dataMonitoring.collect = false;
            let positionFeedbackFilterType;
            let pKp;
            let pKi;
            let pKd;
            let pCtrlStrategy;
            let minPosLmt;
            let maxPosLmt;
            try {
                // Retrieve encoder ordinal values for commutation (ENC_COM) and position (ENC_POS) configurations
                this.notifications$.next('Retrieving encoder configurations...');
                encCom = yield this.client.request.upload(this.deviceRef, 0x2010, 0x0c);
                encPos = yield this.client.request.upload(this.deviceRef, 0x2012, 0x09);
                this.notifications$.next(`Encoder configurations retrieved: ENC_COM = ${encCom}, ENC_POS = ${encPos}`);
                if (encPos === 0) {
                    throw new Error('ENC_POS is set to 0 (no position feedback). A valid position feedback configuration is required.');
                }
                // Ensure the drive is in the "Switch On Disabled" state before any motion commands
                this.notifications$.next('Transitioning to "Switch On Disabled" state...');
                yield this.client.request.transitionToCia402State(this.deviceRef, cia402_1.Cia402State.SWITCH_ON_DISABLED);
                this.notifications$.next('"Switch On Disabled" state reached.');
                // Compute the feed constant required for motion control
                this.notifications$.next('Computing feed constant...');
                const feedConstant = yield (0, rxjs_1.lastValueFrom)(this.client.request.computeFeedConstant(this.deviceRef));
                this.notifications$.next(`Feed constant computed: ${feedConstant}`);
                // Disable the position feedback filter
                this.notifications$.next('Disabling the position feedback filter...');
                positionFeedbackFilterType = yield this.client.request.upload(this.deviceRef, 0x2022, 0x01);
                yield this.client.request.download(this.deviceRef, 0x2022, 0x01, 0);
                this.notifications$.next('Position feedback filter disabled.');
                // Retrieve the encoder resolution based on the ENC_COM configuration
                this.notifications$.next('Reading encoder resolution for commutation configuration...');
                const resCom = yield this.client.request.upload(this.deviceRef, encCom === 1 ? 0x2110 : 0x2112, 0x03);
                this.notifications$.next(`Encoder resolution for commutation configuration: ${resCom}`);
                // Check if the position encoder (ENC_POS) is synchronized with the commutation encoder (ENC_COM).
                // If not, update ENC_POS to match ENC_COM to ensure correct electrical/mechanical alignment.
                // Then adjust software position limits to account for any scaling differences.
                if (encPos !== encCom) {
                    // Variables to hold position limit deltas
                    let minPosDelta = 0;
                    let maxPosDelta = 0;
                    // Read the current software position limits from device
                    this.notifications$.next('Reading software position limits...');
                    minPosLmt = yield this.client.request.upload(this.deviceRef, 0x607d, 0x01);
                    maxPosLmt = yield this.client.request.upload(this.deviceRef, 0x607d, 0x02);
                    this.notifications$.next(`Software position limits: Min = ${minPosLmt}, Max = ${maxPosLmt}`);
                    // Determine if any software position limit is enabled
                    const anyLimitEnabled = minPosLmt !== -2147483648 || maxPosLmt !== 2147483647;
                    if (anyLimitEnabled) {
                        // Read encoder resolution for mechanical position scaling
                        this.notifications$.next('Reading encoder resolution for position configuration...');
                        const resPos = yield this.client.request.upload(this.deviceRef, encPos === 1 ? 0x2110 : 0x2112, 0x03);
                        this.notifications$.next(`Encoder resolution for position configuration: ${resPos}`);
                        // Read current position before ENC_POS update (old scaling)
                        this.notifications$.next('Reading current position actual value...');
                        const posActOld = yield this.client.request.upload(this.deviceRef, 0x6064, 0x00);
                        this.notifications$.next(`Current position actual value: ${posActOld}`);
                        // Calculate delta for min software limit if enabled
                        if (minPosLmt !== -2147483648) {
                            this.notifications$.next('Calculating minimum position limit delta...');
                            minPosDelta = Math.round(((posActOld - minPosLmt) / resPos) * gearRatio * resCom);
                            this.notifications$.next(`Minimum position limit delta: ${minPosDelta}`);
                        }
                        // Calculate delta for max software limit if enabled
                        if (maxPosLmt !== 2147483647) {
                            this.notifications$.next('Calculating maximum position limit delta...');
                            maxPosDelta = Math.round(((maxPosLmt - posActOld) / resPos) * gearRatio * resCom);
                            this.notifications$.next(`Maximum position limit delta: ${maxPosDelta}`);
                        }
                    }
                    // Force an on-demand parameters update to apply the new encoder configuration
                    this.notifications$.next('Encoder configurations do not match. Updating ENC_POS to match ENC_COM...');
                    yield this.client.request.download(this.deviceRef, 0x2012, 0x09, encCom);
                    this.notifications$.next('ENC_POS updated to match ENC_COM.');
                    // Force an on-demand parameters update to apply the new encoder configuration
                    this.notifications$.next('Forcing on-demand parameters update...');
                    yield this.client.request.forceOnDemandParametersUpdate(this.deviceRef);
                    this.notifications$.next('On-demand parameters update forced.');
                    // Adjust software position limits based on calculated deltas (if any)
                    if (minPosDelta !== 0 || maxPosDelta !== 0) {
                        this.notifications$.next('Reading current position actual value...');
                        const posActNew = yield this.client.request.upload(this.deviceRef, 0x6064, 0x00);
                        this.notifications$.next(`Current position actual value: ${posActNew}`);
                        // Adjust minimum position limit if delta exists
                        if (minPosDelta !== 0) {
                            const value = posActNew - minPosDelta;
                            this.notifications$.next(`Adjusting minimum position limit by delta: ${minPosDelta}...`);
                            yield this.client.request.download(this.deviceRef, 0x607d, 0x01, Math.max(value, -2147483648));
                            this.notifications$.next('Minimum position limit adjusted.');
                        }
                        // Adjust maximum position limit if delta exists
                        if (maxPosDelta !== 0) {
                            const value = posActNew + maxPosDelta;
                            this.notifications$.next(`Adjusting maximum position limit by delta: ${maxPosDelta}...`);
                            yield this.client.request.download(this.deviceRef, 0x607d, 0x02, Math.min(value, 2147483647));
                            this.notifications$.next('Maximum position limit adjusted.');
                        }
                    }
                }
                // Calculate adjusted limit range min based on feed constant and encoder resolution
                this.notifications$.next('Calculating limit range  based on feed constant and resolution...');
                // feed constant is not disabled and greater than resolution
                const ratio = feedConstant !== 0xffffffff && feedConstant > resCom ? feedConstant / resCom : 1;
                const rangeLimitMin = Math.round(this.options.rangeLimitMin * ratio);
                this.notifications$.next(`Adjusted range limit: min=${rangeLimitMin}, eff=${rangeLimitEff}`);
                // Retrieve the PID controller parameters
                this.notifications$.next('Retrieving the PID values...');
                [pKp, pKi, pKd, pCtrlStrategy] = (yield this.client.request.uploadMany([
                    [this.deviceRef, 0x2012, 0x01],
                    [this.deviceRef, 0x2012, 0x02],
                    [this.deviceRef, 0x2012, 0x03],
                    [this.deviceRef, 0x2002, 0x00],
                ]));
                this.notifications$.next(`PID values retrieved: Kp=${pKp}, Ki=${pKi}, Kd=${pKd}, CtrlStrategy=${pCtrlStrategy}`);
                // Retrieve the maximum torque value from the drive
                this.notifications$.next('Retrieving the maximum torque value...');
                const maxTorque = yield this.client.request.upload(this.deviceRef, 0x6072, 0x00);
                this.notifications$.next(`Maximum torque value retrieved: ${maxTorque}`);
                // Compute the initial kp value and xi squared based on the retrieved parameters
                this.notifications$.next('Computing the initial kp value and xi squared...');
                let kpInit;
                let xiSquared;
                if (feedConstant === 0xffffffff) {
                    xiSquared = Math.min(maxTorque, 1000 * Math.sqrt(rangeLimitEff / rangeLimitMin)) * rangeLimitEff;
                    kpInit = xiSquared / Math.pow(rangeLimitEff, 2);
                }
                else {
                    xiSquared =
                        ((Math.min(maxTorque, 1000 * Math.sqrt(rangeLimitEff / rangeLimitMin)) * rangeLimitEff) / feedConstant) *
                            resCom;
                    kpInit = xiSquared / Math.pow((rangeLimitEff / feedConstant) * resCom, 2);
                }
                this.notifications$.next(`Initial kp value: ${kpInit}, xi squared: ${xiSquared}`);
                // Set the initial PID controller parameters for the procedure
                this.notifications$.next('Setting initial PID values...');
                yield this.client.request.downloadMany([
                    [this.deviceRef, 0x2012, 1, kpInit],
                    [this.deviceRef, 0x2012, 2, 0],
                    [this.deviceRef, 0x2012, 3, 0],
                    [this.deviceRef, 0x2002, 0, 1],
                ]);
                this.notifications$.next('Initial PID values set.');
                // Force an on-demand parameters update to apply all settings
                this.notifications$.next('Forcing on-demand parameters update...');
                yield this.client.request.forceOnDemandParametersUpdate(this.deviceRef);
                this.notifications$.next('On-demand parameters update forced.');
                // Variable to track if a fault occurred during the procedure
                let hasFault = false;
                // Previous Kp value to minimize downloads
                let prevKp = 0;
                // Get the reference position before starting data monitoring
                this.notifications$.next('Retrieving reference position...');
                const posRef = yield this.client.request.upload(this.deviceRef, 0x6064, 0x00);
                this.notifications$.next(`Reference position: ${posRef}`);
                // Start data monitoring for position actual value (0x6064:00) and update Kp based on position error
                this.notifications$.next('Starting data monitoring and updating Kp based on position error...');
                dataMonitoring
                    .start()
                    .pipe(
                // Continue while the procedure is running and no fault has occurred
                (0, rxjs_1.takeWhile)(() => this.isRunning && !hasFault), 
                // Convert each emitted actual position into a download request to update the position loop Kp,
                // ignoring new emissions until the previous request completes
                (0, rxjs_1.exhaustMap)((values) => {
                    const statusword = values[1];
                    const cia402State = (0, cia402_1.getCia402State)(statusword);
                    if (cia402State === cia402_1.Cia402State.OPERATION_ENABLED) {
                        const positionActualValue = values[2];
                        const e = posRef - positionActualValue;
                        let kp;
                        if (feedConstant === 0xffffffff) {
                            kp = xiSquared / Math.pow(Math.max(rangeLimitEff - Math.abs(e), 1), 2);
                        }
                        else {
                            kp = xiSquared / Math.pow((Math.max(rangeLimitEff - Math.abs(e), 1) / feedConstant) * resCom, 2);
                        }
                        // Update Kp only if it has changed to minimize downloads
                        if (kp !== prevKp) {
                            prevKp = kp;
                            return (0, rxjs_1.from)(this.client.request.download(this.deviceRef, 0x2012, 0x01, kp));
                        }
                    }
                    else if ((0, cia402_1.getCia402State)(statusword) === cia402_1.Cia402State.FAULT_REACTION_ACTIVE ||
                        (0, cia402_1.getCia402State)(statusword) === cia402_1.Cia402State.FAULT) {
                        hasFault = true;
                    }
                    return rxjs_1.EMPTY;
                }))
                    .subscribe({
                    complete: () => {
                        this.notifications$.next('Data monitoring and Kp updating have stopped.');
                    },
                });
                this.notifications$.next('Data monitoring started.');
                // Read the current position and set it as the target position to prevent sudden movement
                this.notifications$.next('Setting current position as target position...');
                const positionActualValue = yield this.client.request.upload(this.deviceRef, 0x6064, 0x00);
                yield this.client.request.download(this.deviceRef, 0x607a, 0x00, positionActualValue);
                this.notifications$.next(`Target position set to ${positionActualValue}.`);
                // Run the chirp signal on the device
                this.notifications$.next('Running the chirp signal...');
                yield (0, rxjs_1.lastValueFrom)(this.client.request.runChirpSignal(this.deviceRef, this.options));
                this.notifications$.next('Chirp signal procedure completed.');
                if (hasFault) {
                    throw new Error('Device entered FAULT state during chirp signal execution.');
                }
                // Stop data monitoring
                this.notifications$.next('Stopping data monitoring...');
                dataMonitoring.stop();
                (_a = this.dataMonitoringSubscription) === null || _a === void 0 ? void 0 : _a.unsubscribe();
                this.notifications$.next('Data monitoring stopped.');
                // Start the system identification procedure without the chirp signal in order to only compute the plant model
                this.notifications$.next('Starting system identification procedure on Motion Master to compute plant model...');
                const deviceRefObj = (0, device_1.makeDeviceRefObj)(this.deviceRef);
                const props = Object.assign(Object.assign({}, deviceRefObj), { skipChirpSignal: true, nextGenSysId: true, durationSeconds: this.options.transitionTime / 1000, endFrequency: this.options.targetFrequency / 1000, startFrequency: this.options.startFrequency / 1000, torqueAmplitude: this.options.targetAmplitude });
                const sysIdStatus = yield (0, rxjs_1.lastValueFrom)(this.client.request.startSystemIdentification(props, 10000));
                if (sysIdStatus.request === 'failed') {
                    throw new Error(`Computing plant model failed: ${(_b = sysIdStatus.error) === null || _b === void 0 ? void 0 : _b.message}`);
                }
                this.notifications$.next('Computing plant model completed.');
            }
            catch (error) {
                this.notifications$.next(`Error: ${error instanceof Error ? error.message : error}`);
                throw error;
            }
            finally {
                // Mark the procedure as not running
                this.isRunning = false;
                // Stop data monitoring
                this.notifications$.next('Stopping data monitoring...');
                dataMonitoring.stop();
                (_c = this.dataMonitoringSubscription) === null || _c === void 0 ? void 0 : _c.unsubscribe();
                this.notifications$.next('Data monitoring stopped.');
                // Ensure the drive is in the "Switch On Disabled" after the procedure
                this.notifications$.next('Transitioning to "Switch On Disabled" state...');
                try {
                    yield this.client.request.transitionToCia402State(this.deviceRef, cia402_1.Cia402State.SWITCH_ON_DISABLED);
                    this.notifications$.next('"Switch On Disabled" state reached.');
                }
                catch (err) {
                    this.notifications$.next(`Failed to transition to "Switch On Disabled": ${err instanceof Error ? err.message : err}`);
                }
                // Restore original ENC_POS configuration if it was changed
                if (encCom !== undefined && encPos !== undefined && encCom !== encPos) {
                    this.notifications$.next(`Restoring original ENC_POS configuration ${encPos}...`);
                    try {
                        yield this.client.request.download(this.deviceRef, 0x2012, 0x09, encPos);
                        this.notifications$.next('ENC_POS configuration restored.');
                    }
                    catch (err) {
                        this.notifications$.next(`Failed to restore ENC_POS: ${err instanceof Error ? err.message : err}`);
                    }
                }
                // Restore original position limits if they were changed
                if (minPosLmt !== undefined && maxPosLmt !== undefined) {
                    this.notifications$.next('Restoring original position limits...');
                    try {
                        yield this.client.request.downloadMany([
                            [this.deviceRef, 0x607d, 0x01, minPosLmt],
                            [this.deviceRef, 0x607d, 0x02, maxPosLmt],
                        ]);
                        this.notifications$.next('Position limits restored.');
                    }
                    catch (err) {
                        this.notifications$.next(`Failed to restore position limits: ${err instanceof Error ? err.message : err}`);
                    }
                }
                // Restore the position feedback filter if it was changed
                if (positionFeedbackFilterType !== undefined && positionFeedbackFilterType !== 0) {
                    this.notifications$.next('Restoring the position feedback filter...');
                    try {
                        yield this.client.request.download(this.deviceRef, 0x2022, 0x01, positionFeedbackFilterType);
                        this.notifications$.next('Position feedback filter restored.');
                    }
                    catch (err) {
                        this.notifications$.next(`Failed to restore position feedback filter: ${err instanceof Error ? err.message : err}`);
                    }
                }
                // Restore the original PID controller parameters
                if (pKp !== undefined && pKi !== undefined && pKd !== undefined && pCtrlStrategy !== undefined) {
                    this.notifications$.next('Restoring original PID values...');
                    try {
                        yield this.client.request.downloadMany([
                            [this.deviceRef, 0x2012, 0x01, pKp],
                            [this.deviceRef, 0x2012, 0x02, pKi],
                            [this.deviceRef, 0x2012, 0x03, pKd],
                            [this.deviceRef, 0x2002, 0x00, pCtrlStrategy],
                        ]);
                        this.notifications$.next('Original PID values restored.');
                    }
                    catch (err) {
                        this.notifications$.next(`Failed to restore PID values: ${err instanceof Error ? err.message : err}`);
                    }
                }
                // Force an on-demand parameters update to apply all restored settings
                this.notifications$.next('Forcing on-demand parameters update...');
                try {
                    yield this.client.request.forceOnDemandParametersUpdate(this.deviceRef);
                    this.notifications$.next('On-demand parameters update forced.');
                }
                catch (err) {
                    this.notifications$.next(`Failed to force on-demand parameters update: ${err instanceof Error ? err.message : err}`);
                }
            }
        });
    }
}
exports.LimitedRangeSystemIdentification = LimitedRangeSystemIdentification;
//# sourceMappingURL=limited-range-system-identification.js.map