/*
 * 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: 3474 $
 * $LastChangedDate:: 2024-11-06 18:00:53 +0100#$
 */
// 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                                                                                               
import at.lcm.x2c.utils.QFormat;
// USERCODE-END:Imports                                                                                                 

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

    @Override
    public void convertMaskToImplementation() throws Exception {

        // Mask Parameter: characteristic                                                                               
        MaskComboBox characteristicMaskVal = 
            (MaskComboBox)this.getMaskParameter("characteristic").getMaskDataType();
        TString characteristicMaskData = 
            (TString)characteristicMaskVal.getData();
        // Mask Parameter: fc1                                                                                          
        MaskDouble fc1MaskVal = 
            (MaskDouble)this.getMaskParameter("fc1").getMaskDataType();
        TNumeric fc1MaskData = 
            (TNumeric)fc1MaskVal.getData();
        // Mask Parameter: fc2                                                                                          
        MaskDouble fc2MaskVal = 
            (MaskDouble)this.getMaskParameter("fc2").getMaskDataType();
        TNumeric fc2MaskData = 
            (TNumeric)fc2MaskVal.getData();
        // Mask Parameter: rc                                                                                           
        MaskDouble rcMaskVal = 
            (MaskDouble)this.getMaskParameter("rc").getMaskDataType();
        TNumeric rcMaskData = 
            (TNumeric)rcMaskVal.getData();
        // Mask Parameter: ts_fact                                                                                      
        MaskDouble ts_factMaskVal = 
            (MaskDouble)this.getMaskParameter("ts_fact").getMaskDataType();
        TNumeric ts_factMaskData = 
            (TNumeric)ts_factMaskVal.getData();

        // Implementation Parameter: b0                                                                                 
        TNumeric b0CtrVal = this.getImplementationParameterData("b0");
        // Implementation Parameter: b1                                                                                 
        TNumeric b1CtrVal = this.getImplementationParameterData("b1");
        // Implementation Parameter: b2                                                                                 
        TNumeric b2CtrVal = this.getImplementationParameterData("b2");
        // Implementation Parameter: a1                                                                                 
        TNumeric a1CtrVal = this.getImplementationParameterData("a1");
        // Implementation Parameter: a2                                                                                 
        TNumeric a2CtrVal = this.getImplementationParameterData("a2");
        // Implementation Parameter: sfrb0                                                                              
        TNumeric sfrb0CtrVal = this.getImplementationParameterData("sfrb0");
        // Implementation Parameter: sfrb1                                                                              
        TNumeric sfrb1CtrVal = this.getImplementationParameterData("sfrb1");
        // Implementation Parameter: sfrb2                                                                              
        TNumeric sfrb2CtrVal = this.getImplementationParameterData("sfrb2");
        // Implementation Parameter: sfra1                                                                              
        TNumeric sfra1CtrVal = this.getImplementationParameterData("sfra1");
        // Implementation Parameter: sfra2                                                                              
        TNumeric sfra2CtrVal = this.getImplementationParameterData("sfra2");
        // Implementation Parameter: in_old                                                                             
        TNumeric in_oldCtrVal = this.getImplementationParameterData("in_old");
        // Implementation Parameter: in_veryold                                                                         
        TNumeric in_veryoldCtrVal = this.getImplementationParameterData("in_veryold");
        // Implementation Parameter: out_old                                                                            
        TNumeric out_oldCtrVal = this.getImplementationParameterData("out_old");
        // Implementation Parameter: out_veryold                                                                        
        TNumeric out_veryoldCtrVal = this.getImplementationParameterData("out_veryold");

// 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 */
        final int BITS = 16;

        /* determine numerator shift factor */
        double sfrb0 = Double.valueOf(QFormat.getQFormat(b0, BITS, true));
        double sfrb1 = Double.valueOf(QFormat.getQFormat(b1, BITS, true));
        double sfrb2 = Double.valueOf(QFormat.getQFormat(b2, BITS, true));
        sfrb0CtrVal.setReal(0, 0, sfrb0);
        sfrb1CtrVal.setReal(0, 0, sfrb1);
        sfrb2CtrVal.setReal(0, 0, sfrb2);

        /* determine denominator shift factor */
        double sfra1 = Double.valueOf(QFormat.getQFormat(a1, BITS, true));
        double sfra2 = Double.valueOf(QFormat.getQFormat(a2, BITS, true));
        sfra1CtrVal.setReal(0, 0, sfra1);
        sfra2CtrVal.setReal(0, 0, sfra2);

        /* set coefficients */
        b0CtrVal.setReal(0, 0, QFormat.getQValue(b0, (int) sfrb0, BITS, true));
        b1CtrVal.setReal(0, 0, QFormat.getQValue(b1, (int) sfrb1, BITS, true));
        b2CtrVal.setReal(0, 0, QFormat.getQValue(b2, (int) sfrb2, BITS, true));
        a1CtrVal.setReal(0, 0, QFormat.getQValue(a1, (int) sfra1, BITS, true));
        a2CtrVal.setReal(0, 0, QFormat.getQValue(a2, (int) sfra2, BITS, true));

// USERCODE-END:ConvMaskToImplementation                                                                                
        this.setImplementationParameterData("b0", b0CtrVal);
        this.setImplementationParameterData("b1", b1CtrVal);
        this.setImplementationParameterData("b2", b2CtrVal);
        this.setImplementationParameterData("a1", a1CtrVal);
        this.setImplementationParameterData("a2", a2CtrVal);
        this.setImplementationParameterData("sfrb0", sfrb0CtrVal);
        this.setImplementationParameterData("sfrb1", sfrb1CtrVal);
        this.setImplementationParameterData("sfrb2", sfrb2CtrVal);
        this.setImplementationParameterData("sfra1", sfra1CtrVal);
        this.setImplementationParameterData("sfra2", sfra2CtrVal);
        this.setImplementationParameterData("in_old", in_oldCtrVal);
        this.setImplementationParameterData("in_veryold", in_veryoldCtrVal);
        this.setImplementationParameterData("out_old", out_oldCtrVal);
        this.setImplementationParameterData("out_veryold", out_veryoldCtrVal);
    }

    @Override
    public void convertImplementationToMask() throws Exception {

        // Mask Parameter: characteristic                                                                               
        MaskComboBox characteristicMaskVal = 
            (MaskComboBox)this.getMaskParameter("characteristic").getMaskDataType();
        TString characteristicMaskData = 
            (TString)characteristicMaskVal.getData();
        // Mask Parameter: fc1                                                                                          
        MaskDouble fc1MaskVal = 
            (MaskDouble)this.getMaskParameter("fc1").getMaskDataType();
        TNumeric fc1MaskData = 
            (TNumeric)fc1MaskVal.getData();
        // Mask Parameter: fc2                                                                                          
        MaskDouble fc2MaskVal = 
            (MaskDouble)this.getMaskParameter("fc2").getMaskDataType();
        TNumeric fc2MaskData = 
            (TNumeric)fc2MaskVal.getData();
        // Mask Parameter: rc                                                                                           
        MaskDouble rcMaskVal = 
            (MaskDouble)this.getMaskParameter("rc").getMaskDataType();
        TNumeric rcMaskData = 
            (TNumeric)rcMaskVal.getData();
        // Mask Parameter: ts_fact                                                                                      
        MaskDouble ts_factMaskVal = 
            (MaskDouble)this.getMaskParameter("ts_fact").getMaskDataType();
        TNumeric ts_factMaskData = 
            (TNumeric)ts_factMaskVal.getData();

        // Implementation Parameter: b0                                                                                 
        TNumeric b0CtrVal = this.getImplementationParameterData("b0");
        // Implementation Parameter: b1                                                                                 
        TNumeric b1CtrVal = this.getImplementationParameterData("b1");
        // Implementation Parameter: b2                                                                                 
        TNumeric b2CtrVal = this.getImplementationParameterData("b2");
        // Implementation Parameter: a1                                                                                 
        TNumeric a1CtrVal = this.getImplementationParameterData("a1");
        // Implementation Parameter: a2                                                                                 
        TNumeric a2CtrVal = this.getImplementationParameterData("a2");
        // Implementation Parameter: sfrb0                                                                              
        TNumeric sfrb0CtrVal = this.getImplementationParameterData("sfrb0");
        // Implementation Parameter: sfrb1                                                                              
        TNumeric sfrb1CtrVal = this.getImplementationParameterData("sfrb1");
        // Implementation Parameter: sfrb2                                                                              
        TNumeric sfrb2CtrVal = this.getImplementationParameterData("sfrb2");
        // Implementation Parameter: sfra1                                                                              
        TNumeric sfra1CtrVal = this.getImplementationParameterData("sfra1");
        // Implementation Parameter: sfra2                                                                              
        TNumeric sfra2CtrVal = this.getImplementationParameterData("sfra2");
        // Implementation Parameter: in_old                                                                             
        TNumeric in_oldCtrVal = this.getImplementationParameterData("in_old");
        // Implementation Parameter: in_veryold                                                                         
        TNumeric in_veryoldCtrVal = this.getImplementationParameterData("in_veryold");
        // Implementation Parameter: out_old                                                                            
        TNumeric out_oldCtrVal = this.getImplementationParameterData("out_old");
        // Implementation Parameter: out_veryold                                                                        
        TNumeric out_veryoldCtrVal = this.getImplementationParameterData("out_veryold");

// USERCODE-BEGIN:ConvImplementationToMask                                                                              
        if(true){
            throw new Exception("Implementation-to-Mask conversion not implemented for Block 'BandstopBiQ', Implementation 'FiP16' in Library 'Filter' due to ambiguous parameters");
        }
// USERCODE-END:ConvImplementationToMask                                                                                
        this.getMaskParameter("characteristic").setValue(characteristicMaskData.getString());
        this.getMaskParameter("fc1").setValue(fc1MaskData.toFullString());
        this.getMaskParameter("fc2").setValue(fc2MaskData.toFullString());
        this.getMaskParameter("rc").setValue(rcMaskData.toFullString());
        this.getMaskParameter("ts_fact").setValue(ts_factMaskData.toFullString());
    }
}
