namespace RobotNet.RobotManager.Services.Simulation.Algorithm;

public class FuzzyLogic
{
    private double Gain_P = 0.5;
    private double Gain_I = 0.01;
    private double DiscreteTimeIntegrator_DSTATE;

    public FuzzyLogic WithGainP(double gainP)
    {
        Gain_P = gainP;
        return this;
    }

    public FuzzyLogic WithGainI(double gainI)
    {
        Gain_I = gainI;
        return this;
    }

    private static double Fuzzy_trapmf(double x, double[] parame)
    {
        double b_y1;
        double y2;
        b_y1 = 0.0;
        y2 = 0.0;
        if (x >= parame[1])
        {
            b_y1 = 1.0;
        }
        if (x < parame[0])
        {
            b_y1 = 0.0;
        }
        if (parame[0] <= x && x < parame[1] && parame[0] != parame[1])
        {
            b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
        }
        if (x <= parame[2])
        {
            y2 = 1.0;
        }
        if (x > parame[3])
        {
            y2 = 0.0;
        }
        if (parame[2] < x && x <= parame[3] && parame[2] != parame[3])
        {
            y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x);
        }
        return b_y1 < y2 ? b_y1 : y2;
    }

    private static double Fuzzy_trimf(double x, double[] parame)
    {
        double y;
        y = 0.0;
        if (parame[0] != parame[1] && parame[0] < x && x < parame[1])
        {
            y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
        }
        if (parame[1] != parame[2] && parame[1] < x && x < parame[2])
        {
            y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x);
        }
        if (x == parame[1])
        {
            y = 1.0;
        }
        return y;
    }

    public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample)
    {
        (double wl, double wr) result = new();
        double[] inputMFCache = new double[10];
        double[] outputMFCache = new double[5];
        double[] outputMFCache_0 = new double[5];
        double[] tmp = new double[3];
        double aggregatedOutputs;
        double rtb_TmpSignalConversionAtSFun_0;
        double rtb_antecedentOutputs_e;
        double sumAntecedentOutputs;
        int ruleID;
        double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5];
        double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10];
        double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9];
        double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25];
        byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
                         4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
                         4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
        byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
        byte[] b_1 = [  1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
                            4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
                            2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
        byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
        double inputMFCache_tmp;
        double inputMFCache_tmp_0;
        double inputMFCache_tmp_1;
        double inputMFCache_tmp_2;
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller' */
        /* SignalConversion generated from: '<S4>/ SFunction ' incorporates:
         *  Constant: '<Root>/w'
         *  DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
         *  Gain: '<Root>/Gain1'
         *  MATLAB Function: '<S1>/Evaluate Rule Antecedents'
         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
         *  SignalConversion generated from: '<S7>/ SFunction '
         *  Sum: '<Root>/Sum'
         */
        DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample;
        rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE;
        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        /* MATLAB Function: '<S1>/Evaluate Rule Antecedents' incorporates:
         *  Constant: '<Root>/v'
         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
         *  SignalConversion generated from: '<S4>/ SFunction '
         */
        sumAntecedentOutputs = 0.0;
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f);
        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache[0] = inputMFCache_tmp;
        tmp[0] = -0.5;
        tmp[1] = 0.0;
        tmp[2] = 0.5;
        inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e);
        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache[2] = inputMFCache_tmp_0;
        tmp[0] = -1.0;
        tmp[1] = -0.5;
        tmp[2] = 0.0;
        inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
        tmp[0] = 0.0;
        tmp[1] = 0.5;
        tmp[2] = 1.0;
        inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
        tmp[0] = 0.0;
        tmp[1] = 0.25;
        tmp[2] = 0.5;
        inputMFCache[5] = Fuzzy_trimf(v, tmp);
        tmp[0] = 0.25;
        tmp[1] = 0.5;
        tmp[2] = 0.75;
        inputMFCache[6] = Fuzzy_trimf(v, tmp);
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache_tmp_1 = Fuzzy_trapmf(v, d);
        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache[7] = inputMFCache_tmp_1;
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache_tmp_2 = Fuzzy_trapmf(v, c);
        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        inputMFCache[8] = inputMFCache_tmp_2;
        tmp[0] = 0.5;
        tmp[1] = 0.75;
        tmp[2] = 1.0;
        inputMFCache[9] = Fuzzy_trimf(v, tmp);
        /* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
        aggregatedOutputs = 0.0;
        outputMFCache[0] = 0.0;
        outputMFCache[1] = 0.25;
        outputMFCache[2] = 0.5;
        outputMFCache[3] = 0.75;
        outputMFCache[4] = 1.0;
        for (ruleID = 0; ruleID < 25; ruleID++)
        {
            /* MATLAB Function: '<S1>/Evaluate Rule Antecedents' */
            rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1];
            sumAntecedentOutputs += rtb_antecedentOutputs_e;
            /* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
            aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e;
        }
        /* MATLAB Function: '<S1>/Defuzzify Outputs' incorporates:
         *  MATLAB Function: '<S1>/Evaluate Rule Antecedents'
         *  MATLAB Function: '<S1>/Evaluate Rule Consequents'
         */
        if (sumAntecedentOutputs == 0.0)
        {
            result.wr = 0.5;
        }
        else
        {
            result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
        }
        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
        /* MATLAB Function: '<S2>/Evaluate Rule Antecedents' incorporates:
         *  Constant: '<Root>/v'
         *  SignalConversion generated from: '<S7>/ SFunction '
         */
        sumAntecedentOutputs = 0.0;
        inputMFCache[0] = inputMFCache_tmp;
        tmp[0] = -0.5;
        tmp[1] = 0.0;
        tmp[2] = 0.5;
        inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
        inputMFCache[2] = inputMFCache_tmp_0;
        tmp[0] = -1.0;
        tmp[1] = -0.5;
        tmp[2] = 0.0;
        inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
        tmp[0] = 0.0;
        tmp[1] = 0.5;
        tmp[2] = 1.0;
        inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
        tmp[0] = 0.0;
        tmp[1] = 0.25;
        tmp[2] = 0.5;
        inputMFCache[5] = Fuzzy_trimf(v, tmp);
        tmp[0] = 0.25;
        tmp[1] = 0.5;
        tmp[2] = 0.75;
        inputMFCache[6] = Fuzzy_trimf(v, tmp);
        inputMFCache[7] = inputMFCache_tmp_1;
        inputMFCache[8] = inputMFCache_tmp_2;
        tmp[0] = 0.5;
        tmp[1] = 0.75;
        tmp[2] = 1.0;
        inputMFCache[9] = Fuzzy_trimf(v, tmp);
        /* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
        aggregatedOutputs = 0.0;
        outputMFCache_0[0] = 0.0;
        outputMFCache_0[1] = 0.25;
        outputMFCache_0[2] = 0.5;
        outputMFCache_0[3] = 0.75;
        outputMFCache_0[4] = 1.0;
        for (ruleID = 0; ruleID < 25; ruleID++)
        {
            /* MATLAB Function: '<S2>/Evaluate Rule Antecedents' */
            rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1];
            sumAntecedentOutputs += rtb_antecedentOutputs_e;
            /* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
            aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] *
              rtb_antecedentOutputs_e;
        }
        /* MATLAB Function: '<S2>/Defuzzify Outputs' incorporates:
         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
         *  MATLAB Function: '<S2>/Evaluate Rule Consequents'
         */
        if (sumAntecedentOutputs == 0.0)
        {
            result.wl = 0.5;
        }
        else
        {
            result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
        }
        return result;
    }
}