/*
 * Copyright (c) 2018, 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.filter;

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

/* USERCODE-BEGIN:Imports */
/* USERCODE-END:Imports */

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

    @Override
    public void convertMaskToImplementation() throws Exception {
        MaskComboBox characteristicMaskVal = (MaskComboBox)this.getMaskParameter("characteristic").getMaskDataType();
        TString characteristicMaskData = (TString)characteristicMaskVal.getData();
        String mp_characteristic = characteristicMaskData.getString();
        MaskDouble fcMaskVal = (MaskDouble)this.getMaskParameter("fc").getMaskDataType();
        TNumeric fcMaskData = (TNumeric)fcMaskVal.getData();
        double mp_fc = fcMaskData.getReal(0, 0);
        MaskDouble rpMaskVal = (MaskDouble)this.getMaskParameter("rp").getMaskDataType();
        TNumeric rpMaskData = (TNumeric)rpMaskVal.getData();
        double mp_rp = rpMaskData.getReal(0, 0);
        MaskDouble rsMaskVal = (MaskDouble)this.getMaskParameter("rs").getMaskDataType();
        TNumeric rsMaskData = (TNumeric)rsMaskVal.getData();
        double mp_rs = rsMaskData.getReal(0, 0);
        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 b2CtrVal = this.getImplementationParameterData("b2");
        double cp_b2 = Double.NaN;
        TNumeric a1CtrVal = this.getImplementationParameterData("a1");
        double cp_a1 = Double.NaN;
        TNumeric a2CtrVal = this.getImplementationParameterData("a2");
        double cp_a2 = 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_characteristic OR characteristicMaskVal (String) */
        /* mp_fc OR fcMaskVal (Number) */
        /* mp_rp OR rpMaskVal (Number) */
        /* mp_rs OR rsMaskVal (Number) */
        /* mp_ts_fact OR ts_factMaskVal (Number) */

        /* USERCODE-BEGIN:ConvMaskToImplementation */
        double fc = Math.abs(Double.valueOf(fcMaskVal.getValue()));
        /* get sample time */
        double ts_fact = Double.valueOf(ts_factMaskVal.getValue()).intValue();
        if (ts_fact <= 0) {
            ts_fact = 1;
        }
        double Ts = ts_fact * this.getDedicatedBlock().getModel().getSampleTime();

        /* get sampling frequency */
        double fs = 1 / Ts;

        /* validate input */
        if ((2 * fc) >= fs) {
            throw new Exception("Wrong Input! Cut-Off frequency was choosen to big! Should be less then fs/2.");
        }

        /* Normalize frequency */
        double fcScaled = (2 * fc) / fs;

        /* ----------------------------------------------------------------------------------------- */
        /* Get analog lowpass prototype */
        /* ----------------------------------------------------------------------------------------- */
        final double HALF_SQRT2 = Math.sqrt(2) / 2;
        final double HALF_SQRT3 = Math.sqrt(3) / 2;
        final double SQRT2 = Math.sqrt(2);
        double z_x = 0;
        double z_y = 0;
        double p_x = 0;
        double p_y = 0;
        double k = 0;
        if (characteristicMaskVal.getValue().equals("Butterworth")) {
            p_x = -HALF_SQRT2;
            p_y = HALF_SQRT2;
            k = 1.0;
        } else if (characteristicMaskVal.getValue().equals("Chebyshev I")) {
            double rp = Math.abs(Double.valueOf(rpMaskVal.getValue()));
            if (rp == 0) {
                throw new Exception("Division by zero! Choose a passband ripple different from zero!");
            } else if (rp < 0) {
                throw new Exception("Negative ripple value given! Enter a positive passband ripple value!");
            }

            /* poles */
            double arg = Math.pow(10, 0.1 * rp) - 1.0;
            double eps = Math.sqrt(arg);
            double x = 1 / eps;
            double mu = 0.5 * Math.log(x + Math.sqrt((x * x) + 1)); // = arcsinh(x)
            double theta = Math.PI / 4;
            /*
             * arg = mu + 1j*theta = x + jy p = -np.sinh(arg) = = -(cos(y)*sinh(x) +j sin(y)*cosh(x))
             */
            p_x = -Math.cos(theta) * Math.sinh(mu);
            p_y = -Math.sin(theta) * Math.cosh(mu);

            /* gain */
            double prod_p = (p_x * p_x) + (p_y * p_y);
            k = prod_p / (Math.sqrt(1 + (eps * eps)));

        } else if (characteristicMaskVal.getValue().equals("Chebyshev II")) {
            double rs = Math.abs(Double.valueOf(rsMaskVal.getValue()));
            if (rs == 0) {
                throw new Exception("Division by zero! Choose a passband ripple different from zero!");
            } else if (rs < 0) {
                throw new Exception("Negative ripple value given! Enter a positive stopband ripple value!");
            }
            double arg = Math.pow(10, 0.1 * rs) - 1.0;
            double de = 1 / Math.sqrt(arg);
            double x = 1 / de;
            double mu = Math.log(x + Math.sqrt((x * x) + 1)) / 2; // = arcsinh(x)

            /* zeros */
            z_x = 0;
            z_y = -SQRT2;

            /* poles */
            p_x = -HALF_SQRT2;
            p_y = HALF_SQRT2;
            p_x = Math.sinh(mu) * p_x;
            p_y = Math.cosh(mu) * p_y;

            /* calculate p = 1/p */
            double scale_p = (p_x * p_x) + (p_y * p_y); /* square of magnitude */
            p_x = p_x / scale_p;
            p_y = -p_y / scale_p;

            /* gain */
            double prod_p = (p_x * p_x) + (p_y * p_y);
            double prod_z = (z_x * z_x) + (z_y * z_y);
            k = prod_p / prod_z;
        } else if (characteristicMaskVal.getValue().equals("Bessel")) {
            /* poles */
            p_x = -1.5;
            p_y = HALF_SQRT3;
            /* gain */
            k = 3.0;
        }

        /* ----------------------------------------------------------------------------------------- */
        /* Pre-warp frequencies for digital filter design */
        /* ----------------------------------------------------------------------------------------- */
        double fsScaled = 2.0;
        double warped = 2 * fsScaled * Math.tan((Math.PI * fcScaled) / fsScaled);

        /* ----------------------------------------------------------------------------------------- */
        /* transform to highpass */
        /* ----------------------------------------------------------------------------------------- */
        double relDegree = 0;

        /* zeros */
        double zHp_x = 0;
        double zHp_y = 0;
        double scale_z = 0;
        if (characteristicMaskVal.getValue().equals("Chebyshev II")) {
            relDegree = 0;
            /*
             * Invert positions radially about unit circle to convert LPF to HPF Scale all points radially from origin
             * to shift cutoff frequency z_hp = warped / z
             */
            scale_z = (z_x * z_x) + (z_y * z_y);
            zHp_x = (warped * z_x) / scale_z;
            zHp_y = (warped * z_y) / scale_z;
        } else {
            relDegree = 2.0; /* =len(p)-len(z) */
            zHp_x = 0;
            zHp_y = 0;
            scale_z = 1;
        }

        /* poles */
        /*
         * Invert positions radially about unit circle to convert LPF to HPF Scale all points radially from origin to
         * shift cutoff frequency p_hp = warped / p
         */
        double scale_p = (p_x * p_x) + (p_y * p_y); /* square of magnitude */
        double pHp_x = (warped * p_x) / scale_p; /* real part of conj poles of highpass */
        double pHp_y = (warped * p_y) / scale_p; /* magnitude of imag. part of conj poles of highpass */

        /* gain */
        /*
         * Cancel out gain change caused by inversion k_hp = k * np.real(np.prod(-z) / np.prod(-p))
         */
        double k_hp = (k * scale_z) / scale_p;

        /* ----------------------------------------------------------------------------------------- */
        /* transform to discrete highpass (bilinear transform) */
        /* ----------------------------------------------------------------------------------------- */
        double fs2 = 2.0 * 2;
        double zz_x = 0;
        double zz_y = 0;
        double nom_x = 0;
        double nom_y = 0;
        double denom_x = 0;
        double denom_y = 0;
        double mag_nom;
        double mag_denom;
        double mag;
        double phase_nom;
        double phase_denom;
        double phase;
        double prod_z = 0;

        /*
         * zeros z_z = (fs2 + z) / (fs2 - z)
         */
        nom_x = fs2 + zHp_x;
        nom_y = zHp_y;
        denom_x = fs2 - zHp_x;
        denom_y = -zHp_y;
        prod_z = (denom_x * denom_x) + (denom_y * denom_y); /* product of conjugate complex number */

        /* rect2pol */
        mag_nom = Math.sqrt(Math.pow(nom_x, 2) + Math.pow(nom_y, 2));
        mag_denom = Math.sqrt(Math.pow(denom_x, 2) + Math.pow(denom_y, 2));
        mag = mag_nom / mag_denom;

        phase_nom = Math.atan2(nom_y, nom_x);
        phase_denom = Math.atan2(denom_y, denom_x);
        phase = phase_nom - phase_denom;

        /* pol2rect */
        zz_x = mag * Math.cos(phase); /* real */
        zz_y = mag * Math.sin(phase); /* imag */

        /* poles */
        /* p_z = (fs2 + p) / (fs2 - p) */
        nom_x = fs2 + pHp_x;
        nom_y = pHp_y;
        denom_x = fs2 - pHp_x;
        denom_y = -pHp_y;

        /* rect2pol */
        mag_nom = Math.sqrt(Math.pow(nom_x, 2) + Math.pow(nom_y, 2));
        mag_denom = Math.sqrt(Math.pow(denom_x, 2) + Math.pow(denom_y, 2));
        mag = mag_nom / mag_denom;

        phase_nom = Math.atan2(nom_y, nom_x);
        phase_denom = Math.atan2(denom_y, denom_x);
        phase = phase_nom - phase_denom;

        /* pol2rect */
        double pz_x = mag * Math.cos(phase); /* real */
        double pz_y = mag * Math.sin(phase); /* imag */

        double prod_p = (denom_x * denom_x) + (denom_y * denom_y); /* product of conjugate complex number */

        /* gain */
        double kz = (k_hp * prod_z) / prod_p;

        /* ----------------------------------------------------------------------------------------- */
        /* transform to proper form */
        /* ----------------------------------------------------------------------------------------- */
        /* numerator coefficients */
        double b0 = kz * 1; /* coefficient to s^2 */
        double b1 = kz * -2.0 * zz_x; /* coefficient to s^1 */
        double b2 = kz * ((zz_x * zz_x) + (zz_y * zz_y)); /* coefficient to s^0 */

        /* denominator coefficients */
        double a0 = 1; /* coefficient to s^2 */
        double a1 = -2.0 * pz_x; /* coefficient to s^1 */
        double a2 = Math.pow(pz_x, 2) + Math.pow(pz_y, 2); /* coefficient to s^0 */

        /* set block parameter = filter coefficients */
        b0CtrVal.setReal(0, 0, b0);
        b1CtrVal.setReal(0, 0, b1);
        b2CtrVal.setReal(0, 0, b2);
        a1CtrVal.setReal(0, 0, a1);
        a2CtrVal.setReal(0, 0, a2);

        /* USERCODE-END:ConvMaskToImplementation */

        /* Implementation Parameter values */
        /* cp_b0 OR b0CtrVal (Value) */
        /* cp_b1 OR b1CtrVal (Value) */
        /* cp_b2 OR b2CtrVal (Value) */
        /* cp_a1 OR a1CtrVal (Value) */
        /* cp_a2 OR a2CtrVal (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_b2)) {
            this.setImplementationParameterData("b2", b2CtrVal);
        } else {
            this.setImplementationParameterData("b2", new TNumeric(cp_b2));
        }
        if (Double.isNaN(cp_a1)) {
            this.setImplementationParameterData("a1", a1CtrVal);
        } else {
            this.setImplementationParameterData("a1", new TNumeric(cp_a1));
        }
        if (Double.isNaN(cp_a2)) {
            this.setImplementationParameterData("a2", a2CtrVal);
        } else {
            this.setImplementationParameterData("a2", new TNumeric(cp_a2));
        }
    }

    @Override
    public void convertImplementationToMask() throws Exception {
        MaskComboBox characteristicMaskVal = (MaskComboBox)this.getMaskParameter("characteristic").getMaskDataType();
        TString characteristicMaskData = (TString)characteristicMaskVal.getData();
        String mp_characteristic = null;
        MaskDouble fcMaskVal = (MaskDouble)this.getMaskParameter("fc").getMaskDataType();
        TNumeric fcMaskData = (TNumeric)fcMaskVal.getData();
        double mp_fc = Double.NaN;
        MaskDouble rpMaskVal = (MaskDouble)this.getMaskParameter("rp").getMaskDataType();
        TNumeric rpMaskData = (TNumeric)rpMaskVal.getData();
        double mp_rp = Double.NaN;
        MaskDouble rsMaskVal = (MaskDouble)this.getMaskParameter("rs").getMaskDataType();
        TNumeric rsMaskData = (TNumeric)rsMaskVal.getData();
        double mp_rs = Double.NaN;
        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 b2CtrVal = this.getImplementationParameterData("b2");
        double cp_b2 = b2CtrVal.getReal(0, 0);
        TNumeric a1CtrVal = this.getImplementationParameterData("a1");
        double cp_a1 = a1CtrVal.getReal(0, 0);
        TNumeric a2CtrVal = this.getImplementationParameterData("a2");
        double cp_a2 = a2CtrVal.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_b2 OR b2CtrVal (Value) */
        /* cp_a1 OR a1CtrVal (Value) */
        /* cp_a2 OR a2CtrVal (Value) */

        /* USERCODE-BEGIN:ConvImplementationToMask */
        if (true) {
            throw new Exception(
                    "Implementation-to-Mask conversion not implemented for Block 'HighpassBiQ', Implementation 'Float32' in Library 'Filter' due to ambiguous parameters");
        }
        /* USERCODE-END:ConvImplementationToMask */

        /* Mask parameter values */
        /* mp_characteristic OR characteristicMaskVal (String) */
        /* mp_fc OR fcMaskVal (Number) */
        /* mp_rp OR rpMaskVal (Number) */
        /* mp_rs OR rsMaskVal (Number) */
        /* mp_ts_fact OR ts_factMaskVal (Number) */

        if (mp_characteristic == null) {
            this.getMaskParameter("characteristic").setValue(characteristicMaskData.getString());
        } else {
            this.getMaskParameter("characteristic").setValue(mp_characteristic);
        }
        if (Double.isNaN(mp_fc)) {
            this.getMaskParameter("fc").setValue(fcMaskData.toFullString());
        } else {
            this.getMaskParameter("fc").setValue(new TNumeric(mp_fc).toFullString());
        }
        if (Double.isNaN(mp_rp)) {
            this.getMaskParameter("rp").setValue(rpMaskData.toFullString());
        } else {
            this.getMaskParameter("rp").setValue(new TNumeric(mp_rp).toFullString());
        }
        if (Double.isNaN(mp_rs)) {
            this.getMaskParameter("rs").setValue(rsMaskData.toFullString());
        } else {
            this.getMaskParameter("rs").setValue(new TNumeric(mp_rs).toFullString());
        }
    }
}
