/*
 * Copyright (c) 2013, Linz Center of Mechatronics GmbH (LCM), web: www.lcm.at
 * All rights reserved.
 */
/*
 * This file is licensed according to the BSD 3-clause license as follows:
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the "Linz Center of Mechatronics GmbH" and "LCM" nor
 *       the names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL "Linz Center of Mechatronics GmbH" BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * This file is part of X2C. web: x2c.lcm.at
 * $LastChangedRevision: 4463 $
 * $LastChangedDate:: 2026-08-17 13:59:56 +0200#$
 */
/* USERCODE-BEGIN:Description */
//*     Description:                                                           *
/* USERCODE-END:Description */

package at.lcm.x2c.library.control;

import at.lcm.x2c.core.structure.*;
import at.lcm.bu21.general.dtypes.*;

/* USERCODE-BEGIN:Imports */
import at.lcm.x2c.utils.QFormat;

/* USERCODE-END:Imports */

@SuppressWarnings("unused")
public class ConvFnc_PIDLimit_FiP32 extends JavaConversionFunction {
    private static final long serialVersionUID = 1L;

    @Override
    public void convertMaskToImplementation() throws Exception {
        MaskDouble KpMaskVal = (MaskDouble)this.getMaskParameter("Kp").getMaskDataType();
        TNumeric KpMaskData = (TNumeric)KpMaskVal.getData();
        double mp_Kp = KpMaskData.getReal(0, 0);
        MaskDouble KiMaskVal = (MaskDouble)this.getMaskParameter("Ki").getMaskDataType();
        TNumeric KiMaskData = (TNumeric)KiMaskVal.getData();
        double mp_Ki = KiMaskData.getReal(0, 0);
        MaskDouble KdMaskVal = (MaskDouble)this.getMaskParameter("Kd").getMaskDataType();
        TNumeric KdMaskData = (TNumeric)KdMaskVal.getData();
        double mp_Kd = KdMaskData.getReal(0, 0);
        MaskDouble fcMaskVal = (MaskDouble)this.getMaskParameter("fc").getMaskDataType();
        TNumeric fcMaskData = (TNumeric)fcMaskVal.getData();
        double mp_fc = fcMaskData.getReal(0, 0);
        MaskComboBox initHandlingMaskVal = (MaskComboBox)this.getMaskParameter("initHandling").getMaskDataType();
        TString initHandlingMaskData = (TString)initHandlingMaskVal.getData();
        String mp_initHandling = initHandlingMaskData.getString();
        MaskDouble ts_factMaskVal = (MaskDouble)this.getMaskParameter("ts_fact").getMaskDataType();
        TNumeric ts_factMaskData = (TNumeric)ts_factMaskVal.getData();
        double mp_ts_fact = ts_factMaskData.getReal(0, 0);

        TNumeric b0CtrVal = this.getImplementationParameterData("b0");
        double cp_b0 = Double.NaN;
        TNumeric b1CtrVal = this.getImplementationParameterData("b1");
        double cp_b1 = Double.NaN;
        TNumeric b0dCtrVal = this.getImplementationParameterData("b0d");
        double cp_b0d = Double.NaN;
        TNumeric b1dCtrVal = this.getImplementationParameterData("b1d");
        double cp_b1d = Double.NaN;
        TNumeric a0dCtrVal = this.getImplementationParameterData("a0d");
        double cp_a0d = Double.NaN;
        TNumeric sfrb0CtrVal = this.getImplementationParameterData("sfrb0");
        double cp_sfrb0 = Double.NaN;
        TNumeric sfrb1CtrVal = this.getImplementationParameterData("sfrb1");
        double cp_sfrb1 = Double.NaN;
        TNumeric sfrdCtrVal = this.getImplementationParameterData("sfrd");
        double cp_sfrd = Double.NaN;
        TNumeric initHandlingCtrVal = this.getImplementationParameterData("initHandling");
        double cp_initHandling = Double.NaN;

        /* Check if sample time factor is an integer value */
        if (Math.rint(mp_ts_fact) != mp_ts_fact) {
            throw new Exception("Sample time factor must be a whole number");
        }
        /* Check if sample time factor is within the valid range (upper limit defined by unsigned 16-bit integer range) */
        if ((mp_ts_fact < 1.0) || (mp_ts_fact > 65535.0)) {
            throw new Exception("Sample time factor must be at least 1 and at most 65535");
        }
        /* Block sample time */
        double bp_ts = mp_ts_fact * this.getDedicatedBlock().getModel().getSampleTime();
        /* Check if Block sample time has a valid value */
        if (bp_ts <= 0.0) {
            throw new Exception("Block sample time must be greater than zero");
        }

        /* ATTENTION: */
        /* The M2I conversion function may either use the mp_* or *MaskVal value to get the Mask Parameter value */
        /* For new or updated implementations, the mp_* value shall be used where possible */
        /* Currently, the mp_* value cannot be used for getting non-scalar Mask Parameter data but will return only the first element for these */
        /* The Implementation Parameter assignment variables cp_* can and shall be used for all Implementation Parameter data types */

        /* INFO: Implementation data assignment for array types when using the cp_* variables */
        /* If the cp_* parameter is a FLASH ARRAY, the cp_* variable is assigned an array type but without an actual array */
        /* The M2I conversion function has to assign an array with the correct size and fill it with content */
        /* If the cp_* parameter is a RAM ARRAY, the cp_* variable is assigned an array with the correct size */
        /* The first element is assigned an invalid value while the other elements are (auto)initialized with zero */
        /* The M2I conversion function has to fill the array with content */

        /* Mask parameter values */
        /* mp_Kp OR KpMaskVal (Number) */
        /* mp_Ki OR KiMaskVal (Number) */
        /* mp_Kd OR KdMaskVal (Number) */
        /* mp_fc OR fcMaskVal (Number) */
        /* mp_initHandling OR initHandlingMaskVal (String) */
        /* mp_ts_fact OR ts_factMaskVal (Number) */

        /* USERCODE-BEGIN:ConvMaskToImplementation */
        double Kp, Ki, Kd;
        double fc, wc;
        double Ts = 0;
        int ts_fact;
        double b0, b1, b0d, b1d, a0d;
        final int BITS = 32;

        // get controller parameter values
        Kp = Double.valueOf(KpMaskVal.getValue());
        Ki = Double.valueOf(KiMaskVal.getValue());
        Kd = Double.valueOf(KdMaskVal.getValue());
        fc = Double.valueOf(fcMaskVal.getValue());
        ts_fact = Double.valueOf(ts_factMaskVal.getValue()).intValue();

        // validate controller parameters
        fc = Math.abs(fc);
        if (ts_fact <= 0) {
            ts_fact = 1;
        }

        // get function of Init input
        if (initHandlingMaskVal.getValue().equals("initialize integral")) {
            // set handling to 0 for init of integral
            initHandlingCtrVal.setReal(0, 0, 0);
        } else if (initHandlingMaskVal.getValue().equals("initialize output")) {
            // set handling to 1 for init of output
            initHandlingCtrVal.setReal(0, 0, 1);
        }
        // calculate temporary variables
        wc = 2 * Math.PI * fc;
        Ts = ts_fact * this.getDedicatedBlock().getModel().getSampleTime();

        // Zero order hold (zoh):
        // G(s) = Kp + Ki/s + Kd*s/(s/w+1)-> G(z) = Kp + Ki*Ts*z/(z-1) + Kd*w*(z-1)/(z-exp(-w*T))
        b0 = Ki * Ts;
        b1 = Kp;
        b0d = -Kd * wc;
        b1d = Kd * wc;
        a0d = -Math.exp(-wc * Ts);

        sfrb0CtrVal.setReal(0, 0, Double.valueOf(QFormat.getQFormat(b0, BITS, true)));
        sfrb1CtrVal.setReal(0, 0, Double.valueOf(QFormat.getQFormat(b1, BITS, true)));
        sfrdCtrVal.setReal(0, 0, Double.valueOf(QFormat.getQFormat(b0d, BITS, true)));
        b0CtrVal.setReal(0, 0, Double.valueOf(QFormat.getQValue(b0, (int) (sfrb0CtrVal.getReal(0, 0)), BITS, true)));
        b1CtrVal.setReal(0, 0, Double.valueOf(QFormat.getQValue(b1, (int) (sfrb1CtrVal.getReal(0, 0)), BITS, true)));
        b0dCtrVal.setReal(0, 0, Double.valueOf(QFormat.getQValue(b0d, (int) (sfrdCtrVal.getReal(0, 0)), BITS, true)));
        b1dCtrVal.setReal(0, 0, Double.valueOf(QFormat.getQValue(b1d, (int) (sfrdCtrVal.getReal(0, 0)), BITS, true)));
        a0dCtrVal.setReal(0, 0, Double.valueOf(QFormat.getQValue(a0d, BITS - 1, BITS, true)));

        /* USERCODE-END:ConvMaskToImplementation */

        /* Implementation Parameter values */
        /* cp_b0 OR b0CtrVal (Value) */
        /* cp_b1 OR b1CtrVal (Value) */
        /* cp_b0d OR b0dCtrVal (Value) */
        /* cp_b1d OR b1dCtrVal (Value) */
        /* cp_a0d OR a0dCtrVal (Value) */
        /* cp_sfrb0 OR sfrb0CtrVal (Value) */
        /* cp_sfrb1 OR sfrb1CtrVal (Value) */
        /* cp_sfrd OR sfrdCtrVal (Value) */
        /* cp_initHandling OR initHandlingCtrVal (Value) */

        if (Double.isNaN(cp_b0)) {
            this.setImplementationParameterData("b0", b0CtrVal);
        } else {
            this.setImplementationParameterData("b0", new TNumeric(cp_b0));
        }
        if (Double.isNaN(cp_b1)) {
            this.setImplementationParameterData("b1", b1CtrVal);
        } else {
            this.setImplementationParameterData("b1", new TNumeric(cp_b1));
        }
        if (Double.isNaN(cp_b0d)) {
            this.setImplementationParameterData("b0d", b0dCtrVal);
        } else {
            this.setImplementationParameterData("b0d", new TNumeric(cp_b0d));
        }
        if (Double.isNaN(cp_b1d)) {
            this.setImplementationParameterData("b1d", b1dCtrVal);
        } else {
            this.setImplementationParameterData("b1d", new TNumeric(cp_b1d));
        }
        if (Double.isNaN(cp_a0d)) {
            this.setImplementationParameterData("a0d", a0dCtrVal);
        } else {
            this.setImplementationParameterData("a0d", new TNumeric(cp_a0d));
        }
        if (Double.isNaN(cp_sfrb0)) {
            this.setImplementationParameterData("sfrb0", sfrb0CtrVal);
        } else {
            this.setImplementationParameterData("sfrb0", new TNumeric(cp_sfrb0));
        }
        if (Double.isNaN(cp_sfrb1)) {
            this.setImplementationParameterData("sfrb1", sfrb1CtrVal);
        } else {
            this.setImplementationParameterData("sfrb1", new TNumeric(cp_sfrb1));
        }
        if (Double.isNaN(cp_sfrd)) {
            this.setImplementationParameterData("sfrd", sfrdCtrVal);
        } else {
            this.setImplementationParameterData("sfrd", new TNumeric(cp_sfrd));
        }
        if (Double.isNaN(cp_initHandling)) {
            this.setImplementationParameterData("initHandling", initHandlingCtrVal);
        } else {
            this.setImplementationParameterData("initHandling", new TNumeric(cp_initHandling));
        }
    }

    @Override
    public void convertImplementationToMask() throws Exception {
        MaskDouble KpMaskVal = (MaskDouble)this.getMaskParameter("Kp").getMaskDataType();
        TNumeric KpMaskData = (TNumeric)KpMaskVal.getData();
        double mp_Kp = Double.NaN;
        MaskDouble KiMaskVal = (MaskDouble)this.getMaskParameter("Ki").getMaskDataType();
        TNumeric KiMaskData = (TNumeric)KiMaskVal.getData();
        double mp_Ki = Double.NaN;
        MaskDouble KdMaskVal = (MaskDouble)this.getMaskParameter("Kd").getMaskDataType();
        TNumeric KdMaskData = (TNumeric)KdMaskVal.getData();
        double mp_Kd = Double.NaN;
        MaskDouble fcMaskVal = (MaskDouble)this.getMaskParameter("fc").getMaskDataType();
        TNumeric fcMaskData = (TNumeric)fcMaskVal.getData();
        double mp_fc = Double.NaN;
        MaskComboBox initHandlingMaskVal = (MaskComboBox)this.getMaskParameter("initHandling").getMaskDataType();
        TString initHandlingMaskData = (TString)initHandlingMaskVal.getData();
        String mp_initHandling = null;
        MaskDouble ts_factMaskVal = (MaskDouble)this.getMaskParameter("ts_fact").getMaskDataType();
        TNumeric ts_factMaskData = (TNumeric)ts_factMaskVal.getData();
        double mp_ts_fact = ts_factMaskData.getReal(0, 0);

        TNumeric b0CtrVal = this.getImplementationParameterData("b0");
        double cp_b0 = b0CtrVal.getReal(0, 0);
        TNumeric b1CtrVal = this.getImplementationParameterData("b1");
        double cp_b1 = b1CtrVal.getReal(0, 0);
        TNumeric b0dCtrVal = this.getImplementationParameterData("b0d");
        double cp_b0d = b0dCtrVal.getReal(0, 0);
        TNumeric b1dCtrVal = this.getImplementationParameterData("b1d");
        double cp_b1d = b1dCtrVal.getReal(0, 0);
        TNumeric a0dCtrVal = this.getImplementationParameterData("a0d");
        double cp_a0d = a0dCtrVal.getReal(0, 0);
        TNumeric sfrb0CtrVal = this.getImplementationParameterData("sfrb0");
        double cp_sfrb0 = sfrb0CtrVal.getReal(0, 0);
        TNumeric sfrb1CtrVal = this.getImplementationParameterData("sfrb1");
        double cp_sfrb1 = sfrb1CtrVal.getReal(0, 0);
        TNumeric sfrdCtrVal = this.getImplementationParameterData("sfrd");
        double cp_sfrd = sfrdCtrVal.getReal(0, 0);
        TNumeric initHandlingCtrVal = this.getImplementationParameterData("initHandling");
        double cp_initHandling = initHandlingCtrVal.getReal(0, 0);

        /* Check if sample time factor is an integer value */
        if (Math.rint(mp_ts_fact) != mp_ts_fact) {
            throw new Exception("Sample time factor must be a whole number");
        }
        /* Check if sample time factor is within the valid range (upper limit defined by unsigned 16-bit integer range) */
        if ((mp_ts_fact < 1.0) || (mp_ts_fact > 65535.0)) {
            throw new Exception("Sample time factor must be at least 1 and at most 65535");
        }
        /* Block sample time */
        double bp_ts = mp_ts_fact * this.getDedicatedBlock().getModel().getSampleTime();
        /* Check if Block sample time has a valid value */
        if (bp_ts <= 0.0) {
            throw new Exception("Block sample time must be greater than zero");
        }

        /* ATTENTION: */
        /* The I2M conversion function may either use the cp_* or *CtrlVal value to get the Implementation Parameter value */
        /* For new or updated implementations, the cp_* value shall be used for all Implementation Parameter data types */
        /* The Mask Parameter assignment variables cp_* shall be used where possible */
        /* Currently, the mp_* variable assignment value cannot be used for setting non-scalar Mask Parameter data */

        /* Implementation Parameter values */
        /* cp_b0 OR b0CtrVal (Value) */
        /* cp_b1 OR b1CtrVal (Value) */
        /* cp_b0d OR b0dCtrVal (Value) */
        /* cp_b1d OR b1dCtrVal (Value) */
        /* cp_a0d OR a0dCtrVal (Value) */
        /* cp_sfrb0 OR sfrb0CtrVal (Value) */
        /* cp_sfrb1 OR sfrb1CtrVal (Value) */
        /* cp_sfrd OR sfrdCtrVal (Value) */
        /* cp_initHandling OR initHandlingCtrVal (Value) */

        /* USERCODE-BEGIN:ConvImplementationToMask */
        double Ts = 0;
        int ts_fact;
        double Ki, Kp, Kd, wc, b0, b1, b0d, a0d, sfr0, sfr1, sfrd;
        final int BITS = 32;

        // get controller parameter
        b0 = b0CtrVal.getReal(0, 0);
        b1 = b1CtrVal.getReal(0, 0);
        b0d = b0dCtrVal.getReal(0, 0);
        a0d = a0dCtrVal.getReal(0, 0);
        sfr0 = sfrb0CtrVal.getReal(0, 0);
        sfr1 = sfrb1CtrVal.getReal(0, 0);
        sfrd = sfrdCtrVal.getReal(0, 0);

        // calculate controller coefficients
        b0 = QFormat.getDecValue((long) b0, (int) sfr0, BITS, true);
        b1 = QFormat.getDecValue((long) b1, (int) sfr1, BITS, true);
        b0d = QFormat.getDecValue((long) b0d, (int) sfrd, BITS, true);
        a0d = QFormat.getDecValue((long) a0d, BITS - 1, BITS, true);

        // get sample time information from model (sample time information can't be obtained from target)
        ts_fact = Double.valueOf(ts_factMaskVal.getValue()).intValue();
        Ts = ts_fact * this.getDedicatedBlock().getModel().getSampleTime();

        // Zero order hold (zoh):
        Ki = b0 / Ts;
        Kp = b1;
        wc = -Math.log(-a0d) / Ts;
        Kd = -b0d / wc;

        // set mask parameter
        KpMaskData.setReal(0, 0, Kp);
        KiMaskData.setReal(0, 0, Ki);
        KdMaskData.setReal(0, 0, Kd);
        fcMaskData.setReal(0, 0, wc / (2 * Math.PI));

        /* USERCODE-END:ConvImplementationToMask */

        /* Mask parameter values */
        /* mp_Kp OR KpMaskVal (Number) */
        /* mp_Ki OR KiMaskVal (Number) */
        /* mp_Kd OR KdMaskVal (Number) */
        /* mp_fc OR fcMaskVal (Number) */
        /* mp_initHandling OR initHandlingMaskVal (String) */
        /* mp_ts_fact OR ts_factMaskVal (Number) */

        if (Double.isNaN(mp_Kp)) {
            this.getMaskParameter("Kp").setValue(KpMaskData.toFullString());
        } else {
            this.getMaskParameter("Kp").setValue(new TNumeric(mp_Kp).toFullString());
        }
        if (Double.isNaN(mp_Ki)) {
            this.getMaskParameter("Ki").setValue(KiMaskData.toFullString());
        } else {
            this.getMaskParameter("Ki").setValue(new TNumeric(mp_Ki).toFullString());
        }
        if (Double.isNaN(mp_Kd)) {
            this.getMaskParameter("Kd").setValue(KdMaskData.toFullString());
        } else {
            this.getMaskParameter("Kd").setValue(new TNumeric(mp_Kd).toFullString());
        }
        if (Double.isNaN(mp_fc)) {
            this.getMaskParameter("fc").setValue(fcMaskData.toFullString());
        } else {
            this.getMaskParameter("fc").setValue(new TNumeric(mp_fc).toFullString());
        }
        if (mp_initHandling == null) {
            this.getMaskParameter("initHandling").setValue(initHandlingMaskData.getString());
        } else {
            this.getMaskParameter("initHandling").setValue(mp_initHandling);
        }
    }
}
