/*
 * 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_BandstopBiQ_Float64 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 fc1MaskVal = (MaskDouble)this.getMaskParameter("fc1").getMaskDataType();
        TNumeric fc1MaskData = (TNumeric)fc1MaskVal.getData();
        double mp_fc1 = fc1MaskData.getReal(0, 0);
        MaskDouble fc2MaskVal = (MaskDouble)this.getMaskParameter("fc2").getMaskDataType();
        TNumeric fc2MaskData = (TNumeric)fc2MaskVal.getData();
        double mp_fc2 = fc2MaskData.getReal(0, 0);
        MaskDouble rcMaskVal = (MaskDouble)this.getMaskParameter("rc").getMaskDataType();
        TNumeric rcMaskData = (TNumeric)rcMaskVal.getData();
        double mp_rc = rcMaskData.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_fc1 OR fc1MaskVal (Number) */
        /* mp_fc2 OR fc2MaskVal (Number) */
        /* mp_rc OR rcMaskVal (Number) */
        /* mp_ts_fact OR ts_factMaskVal (Number) */

        /* USERCODE-BEGIN:ConvMaskToImplementation */
        double fc1 = Math.abs(Double.valueOf(fc1MaskVal.getValue()));
        double fc2 = Math.abs(Double.valueOf(fc2MaskVal.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 (fc1 >= fc2) {
            throw new Exception(
                    "Wrong Input! lower cut-off frequency happens to be greater than upper cut-off frequency!");
        }
        if ((2 * fc2) >= fs) {
            throw new Exception("Wrong Input! Upper cut-off frequency was choosen to big! Should be less then fs/2.");
        }

        /* -------------- Normalize frequency */
        double fcScaled1 = (2 * fc1) / fs;
        double fcScaled2 = (2 * fc2) / fs;

        /* -------------- Get analog lowpass prototype with function */
        double z[];
        double p_x = 0;
        double p_y = 0;
        double k = 0;
        if ((characteristicMaskVal.getValue().equals("Butterworth"))
                || (characteristicMaskVal.getValue().equals("Bessel"))) {
            /* poles */
            p_x = -1;
            p_y = -0;
            /* gain */
            k = 1.0;
        } else if (characteristicMaskVal.getValue().equals("Chebyshev")) {
            double rp = Math.abs(Double.valueOf(rcMaskVal.getValue()));
            if (rp == 0) {
                throw new Exception("Division by zero! Choose maximum passband ripple different from zero!");
            }

            /* poles */
            double arg = Math.pow(10, 0.1 * rp) - 1.0;
            double eps = Math.sqrt(arg);
            double x = 1 / eps;
            double mu = Math.log(x + Math.sqrt((x * x) + 1)); // = arcsinh(x)
            double theta = 0;
            /*
             * 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 */
            k = -p_x;
        }

        /* -------------- Pre-warp frequencies for digital filter design */
        double fsScaled = 2.0;
        double warped1 = 2 * fsScaled * Math.tan((Math.PI * fcScaled1) / fsScaled);
        double warped2 = 2 * fsScaled * Math.tan((Math.PI * fcScaled2) / fsScaled);
        double bw = warped2 - warped1; /* bandwidth */
        double wo = Math.sqrt(warped1 * warped2); /* center frequency */

        /* -------------- transform to bandstop */
        double scale_p = (p_x * p_x) + (p_y * p_y); /* square of magnitude */
        double p_hp_x = ((bw / 2) * p_x) / scale_p;
        double p_hp_y = 0; /* p_hp_y = bw/2 * p_y/scale_p; */

        /* zeros */
        double z_bs_x = 0;
        double z_bs_y = wo;
        /* poles */
        double p_bs_x = p_hp_x;
        double arg = (p_hp_x * p_hp_x) - (wo * wo);
        double p_bs_y = Math.sqrt(Math.abs(arg));
        /* gain */
        double prod_z = 1; // analog lowpass prototype has no zeros
        double prod_p = -p_x; // analog lowpass prototype (N=1) has only one real pole.
        double k_bs = (k * prod_z) / prod_p;

        /* -------------- transform to discrete bandstop (bilinear transform) */
        double fs2 = 2.0 * 2;

        /* Bilinear transform of zeros always results in */
        /* p_z = (fs2 + zBs) / (fs2 - zBs) */
        double zz_x = 0;
        double zz_y = 0;

        /* Bilinear transform of poles */
        /* p_z = (fs2 + pBs) / (fs2 - pBs) */
        double pz_x = 0;
        double pz_y = 0;

        /* zeros */
        double nom_x = fs2 + z_bs_x;
        double nom_y = z_bs_y;
        double denom_x = fs2 - z_bs_x;
        double denom_y = -z_bs_y;

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

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

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

        prod_z = (denom_x * denom_x) + (denom_y * denom_y);

        /* poles */
        nom_x = fs2 + p_bs_x;
        nom_y = p_bs_y;
        denom_x = fs2 - p_bs_x;
        denom_y = -p_bs_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 */
        pz_x = mag * Math.cos(phase); /* real */
        pz_y = mag * Math.sin(phase); /* imag */

        prod_p = (denom_x * denom_x) + (denom_y * denom_y); /* product of conj complex number */
        double kz = (k_bs * prod_z) / prod_p;

        /* -------------- transform to proper form (=ba) */
        /* numerator coefficients */
        double b0 = kz * 1; /* coefficient to s^2 */
        double b1 = kz * -2 * zz_x; /* coefficient to s^1 */
        double b2 = (kz * Math.pow(zz_x, 2)) + Math.pow(zz_y, 2); /* 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 fc1MaskVal = (MaskDouble)this.getMaskParameter("fc1").getMaskDataType();
        TNumeric fc1MaskData = (TNumeric)fc1MaskVal.getData();
        double mp_fc1 = Double.NaN;
        MaskDouble fc2MaskVal = (MaskDouble)this.getMaskParameter("fc2").getMaskDataType();
        TNumeric fc2MaskData = (TNumeric)fc2MaskVal.getData();
        double mp_fc2 = Double.NaN;
        MaskDouble rcMaskVal = (MaskDouble)this.getMaskParameter("rc").getMaskDataType();
        TNumeric rcMaskData = (TNumeric)rcMaskVal.getData();
        double mp_rc = 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 'BandstopBiQ', Implementation 'Float64' in Library 'Filter' due to ambiguous parameters");
        }
        /* USERCODE-END:ConvImplementationToMask */

        /* Mask parameter values */
        /* mp_characteristic OR characteristicMaskVal (String) */
        /* mp_fc1 OR fc1MaskVal (Number) */
        /* mp_fc2 OR fc2MaskVal (Number) */
        /* mp_rc OR rcMaskVal (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_fc1)) {
            this.getMaskParameter("fc1").setValue(fc1MaskData.toFullString());
        } else {
            this.getMaskParameter("fc1").setValue(new TNumeric(mp_fc1).toFullString());
        }
        if (Double.isNaN(mp_fc2)) {
            this.getMaskParameter("fc2").setValue(fc2MaskData.toFullString());
        } else {
            this.getMaskParameter("fc2").setValue(new TNumeric(mp_fc2).toFullString());
        }
        if (Double.isNaN(mp_rc)) {
            this.getMaskParameter("rc").setValue(rcMaskData.toFullString());
        } else {
            this.getMaskParameter("rc").setValue(new TNumeric(mp_rc).toFullString());
        }
    }
}
