namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;

public class FuzzyLogic
{
    // ============================================================================
    // FUZZY LOGIC CONTROLLER - PHIÊN BẢN GỐC ĐÃ TỔ CHỨC LẠI
    // Dải đầu ra: 0.0 - 1.0 | Độ phân giải: 5 mức | Số luật: 25 (5x5)
    // ============================================================================

    // ============================================================================
    // PHẦN 1: CÁC HÀM MEMBERSHIP (HÀM THUỘC)
    // ============================================================================

    /// <summary>
    /// Hàm thuộc hình thang (Trapezoidal Membership Function)
    /// Dạng hình thang với 4 điểm: [left_base, left_top, right_top, right_base]
    ///
    ///        left_top ______ right_top
    ///              /          \
    ///             /            \
    ///   _________/              \_________
    /// left_base                  right_base
    /// 
    /// </summary>
    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
    /// <param name="left_base">Điểm bắt đầu hình thang (μ = 0)</param>
    /// <param name="left_top">Điểm bắt đầu vùng phẳng trên (μ = 1)</param>
    /// <param name="right_top">Điểm kết thúc vùng phẳng trên (μ = 1)</param>
    /// <param name="right_base">Điểm kết thúc hình thang (μ = 0)</param>
    /// <returns>Độ thuộc trong khoảng [0, 1]</returns>
    private static double Fuzzy_trapmf(double inputValue, double left_base, double left_top, double right_top, double right_base)
    {
        double membership = 0.0;

        // Trường hợp 1: Nằm ngoài phía trái hình thang
        if (inputValue <= left_base)
        {
            membership = 0.0;
        }
        // Trường hợp 2: Nằm trên cạnh tăng dần (trái)
        else if (inputValue > left_base && inputValue < left_top)
        {
            if (left_top != left_base)  // Tránh chia cho 0
            {
                membership = (inputValue - left_base) / (left_top - left_base);
            }
        }
        // Trường hợp 3: Nằm trên vùng phẳng (đỉnh)
        else if (inputValue >= left_top && inputValue <= right_top)
        {
            membership = 1.0;
        }
        // Trường hợp 4: Nằm trên cạnh giảm dần (phải)
        else if (inputValue > right_top && inputValue < right_base)
        {
            if (right_base != right_top)  // Tránh chia cho 0
            {
                membership = (right_base - inputValue) / (right_base - right_top);
            }
        }
        // Trường hợp 5: Nằm ngoài phía phải hình thang
        else if (inputValue >= right_base)
        {
            membership = 0.0;
        }

        return membership;
    }

    /// <summary>
    /// Hàm thuộc tam giác (Triangular Membership Function)
    /// Dạng tam giác với 3 điểm: [left, peak, right]
    ///
    ///           peak
    ///           /\
    ///          /  \
    ///         /    \
    ///   _____/      \_____
    ///      left      right
    /// 
    /// </summary>
    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
    /// <param name="left">Điểm bắt đầu tam giác (μ = 0)</param>
    /// <param name="peak">Điểm đỉnh tam giác (μ = 1)</param>
    /// <param name="right">Điểm kết thúc tam giác (μ = 0)</param>
    /// <returns>Độ thuộc trong khoảng [0, 1]</returns>
    private static double Fuzzy_trimf(double inputValue, double left, double peak, double right)
    {
        double membership = 0.0;

        // Trường hợp 1: Nằm ngoài phía trái tam giác
        if (inputValue <= left)
        {
            membership = 0.0;
        }
        // Trường hợp 2: Nằm trên cạnh tăng dần (trái)
        else if (inputValue > left && inputValue < peak)
        {
            if (peak != left)  // Tránh chia cho 0
            {
                membership = (inputValue - left) / (peak - left);
            }
        }
        // Trường hợp 3: Đúng tại đỉnh tam giác
        else if (inputValue == peak)
        {
            membership = 1.0;
        }
        // Trường hợp 4: Nằm trên cạnh giảm dần (phải)
        else if (inputValue > peak && inputValue < right)
        {
            if (right != peak)  // Tránh chia cho 0
            {
                membership = (right - inputValue) / (right - peak);
            }
        }
        // Trường hợp 5: Nằm ngoài phía phải tam giác
        else if (inputValue >= right)
        {
            membership = 0.0;
        }

        return membership;
    }

    // ============================================================================
    // PHẦN 2: FUZZIFICATION (MỜ HÓA ĐẦU VÀO)
    // ============================================================================

    /// <summary>
    /// Mờ hóa tín hiệu PI thành 5 tập mờ
    /// </summary>
    /// <param name="piSignal">Tín hiệu đầu ra của bộ PI</param>
    /// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 0-4)</param>
    private static void FuzzifyPISignal(double piSignal, double[] membershipValues)
    {
        // 1. NB (Negative Big): [-∞, -∞, -1.0, -0.5]
        membershipValues[0] = Fuzzy_trapmf(piSignal, -1.0E+10, -1.0E+10, -1.0, -0.5);

        // 2. Z (Zero): [-0.5, 0.0, 0.5]
        membershipValues[1] = Fuzzy_trimf(piSignal, -0.5, 0.0, 0.5);

        // 3. PB (Positive Big): [0.5, 1.0, +∞, +∞]
        membershipValues[2] = Fuzzy_trapmf(piSignal, 0.5, 1.0, 1.0E+10, 1.0E+10);

        // 4. NM (Negative Medium): [-1.0, -0.5, 0.0]
        membershipValues[3] = Fuzzy_trimf(piSignal, -1.0, -0.5, 0.0);

        // 5. PM (Positive Medium): [0.0, 0.5, 1.0]
        membershipValues[4] = Fuzzy_trimf(piSignal, 0.0, 0.5, 1.0);
    }

    /// <summary>
    /// Mờ hóa vận tốc V thành 11 tập mờ (dải 0.0 - 3.0)
    /// Độ phân giải: 0.3 m/s - Cân bằng tốt giữa độ chính xác và hiệu suất
    /// </summary>
    /// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
    /// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 5-15)</param>
    private static void FuzzifyVelocity(double velocity, double[] membershipValues)
    {
        // 11 tập mờ phân bố đều từ 0.0 đến 3.0
        // Khoảng cách giữa các đỉnh: 3.0 / 10 = 0.3 m/s

        // 1. VVS (Very Very Slow): [-∞, -∞, 0.0, 0.3]
        membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.3);

        // 2. VS (Very Slow): [0.0, 0.3, 0.6]
        membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.3, 0.6);

        // 3. S- (Slow Low): [0.3, 0.6, 0.9]
        membershipValues[7] = Fuzzy_trimf(velocity, 0.3, 0.6, 0.9);

        // 4. S (Slow): [0.6, 0.9, 1.2]
        membershipValues[8] = Fuzzy_trimf(velocity, 0.6, 0.9, 1.2);

        // 5. S+ (Slow High): [0.9, 1.2, 1.5]
        membershipValues[9] = Fuzzy_trimf(velocity, 0.9, 1.2, 1.5);

        // 6. M (Medium): [1.2, 1.5, 1.8]
        membershipValues[10] = Fuzzy_trimf(velocity, 1.2, 1.5, 1.8);

        // 7. F- (Fast Low): [1.5, 1.8, 2.1]
        membershipValues[11] = Fuzzy_trimf(velocity, 1.5, 1.8, 2.1);

        // 8. F (Fast): [1.8, 2.1, 2.4]
        membershipValues[12] = Fuzzy_trimf(velocity, 1.8, 2.1, 2.4);

        // 9. F+ (Fast High): [2.1, 2.4, 2.7]
        membershipValues[13] = Fuzzy_trimf(velocity, 2.1, 2.4, 2.7);

        // 10. VF (Very Fast): [2.4, 2.7, 3.0]
        membershipValues[14] = Fuzzy_trimf(velocity, 2.4, 2.7, 3.0);

        // 11. VVF (Very Very Fast): [2.7, 3.0, +∞, +∞]
        membershipValues[15] = Fuzzy_trapmf(velocity, 2.7, 3.0, 1.0E+9, 1.0E+9);
    }

    // ============================================================================
    // PHẦN 3: RULE EVALUATION (ĐÁNH GIÁ LUẬT MỜ)
    // ============================================================================

    /// <summary>
    /// Đánh giá luật mờ cho một bộ điều khiển
    /// </summary>
    /// <param name="inputMembershipValues">Độ thuộc của các đầu vào (16 giá trị: 5 PI + 11 V)</param>
    /// <param name="ruleAntecedentIndices">Ma trận chỉ số tiền đề (numRules * 2 phần tử)</param>
    /// <param name="ruleConsequentIndices">Ma trận chỉ số hệ quả (numRules phần tử)</param>
    /// <param name="outputSingletons">Các giá trị singleton đầu ra</param>
    /// <param name="numRules">Số lượng luật</param>
    /// <returns>(weightedSum: tổng có trọng số, totalWeight: tổng trọng số)</returns>
    private static (double weightedSum, double totalWeight) EvaluateRules(
        double[] inputMembershipValues,
        byte[] ruleAntecedentIndices,
        byte[] ruleConsequentIndices,
        double[] outputSingletons,
        int numRules)
    {
        const int VELOCITY_OFFSET = 5;  // Chỉ số bắt đầu của V trong inputMembershipValues

        double weightedSum = 0.0;
        double totalWeight = 0.0;

        for (int ruleIndex = 0; ruleIndex < numRules; ruleIndex++)
        {
            // Lấy chỉ số tập mờ cho PI (từ 1-5, cần trừ 1 để thành 0-4)
            int piMembershipIndex = ruleAntecedentIndices[ruleIndex] - 1;

            // Lấy chỉ số tập mờ cho Velocity (từ 1-11, cộng offset)
            int velocityMembershipIndex = ruleAntecedentIndices[ruleIndex + numRules] + VELOCITY_OFFSET - 1;

            // Tính độ kích hoạt của luật (AND operator = phép nhân)
            double ruleActivation = inputMembershipValues[piMembershipIndex]
                                   * inputMembershipValues[velocityMembershipIndex];

            // Lấy giá trị singleton đầu ra tương ứng (từ 1-11, cần trừ 1)
            int outputIndex = ruleConsequentIndices[ruleIndex] - 1;
            double outputValue = outputSingletons[outputIndex];

            // Tích lũy tổng trọng số và tổng có trọng số
            totalWeight += ruleActivation;
            weightedSum += outputValue * ruleActivation;
        }

        return (weightedSum, totalWeight);
    }

    // ============================================================================
    // PHẦN 4: DEFUZZIFICATION (GIẢI MỜ ĐẦU RA)
    // ============================================================================

    /// <summary>
    /// Giải mờ bằng phương pháp trọng tâm (Weighted Average / Center of Gravity)
    /// Công thức: output = Σ(singleton_i × weight_i) / Σ(weight_i)
    /// </summary>
    /// <param name="weightedSum">Tổng đầu ra có trọng số</param>
    /// <param name="totalWeight">Tổng trọng số của tất cả các luật</param>
    /// <param name="defaultValue">Giá trị mặc định nếu totalWeight = 0</param>
    /// <returns>Giá trị đầu ra rõ (crisp output)</returns>
    private static double Defuzzify(double weightedSum, double totalWeight, double defaultValue = 0.5)
    {
        // Nếu không có luật nào được kích hoạt, trả về giá trị mặc định
        if (totalWeight == 0.0)
        {
            return defaultValue;
        }
        else
        {
            // Tính trọng tâm: output = weightedSum / totalWeight
            return weightedSum / totalWeight;
        }
    }

    // ============================================================================
    // PHẦN 5: BẢNG LUẬT VÀ CẤU HÌNH
    // ============================================================================

    /// <summary>
    /// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel)
    /// 110 phần tử: 55 cho PI + 55 cho Velocity
    /// Hệ thống: 5 tập mờ PI × 11 tập mờ V = 55 luật
    /// Giá trị từ 1-5 cho PI, 1-11 cho V
    /// </summary>
    private static readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
    [
        // 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
        // Mỗi tập PI lặp lại 11 lần (cho 11 mức vận tốc)
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,  // Luật 1-11:  PI = NB (tập 1)
        2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  // Luật 12-22: PI = Z  (tập 2)
        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,  // Luật 23-33: PI = PB (tập 3)
        4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,  // Luật 34-44: PI = NM (tập 4)
        5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,  // Luật 45-55: PI = PM (tập 5)
    
        // 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
        // Lặp lại theo pattern: VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 1-11:  V = VVS đến VVF
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 12-22: V = VVS đến VVF
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 23-33: V = VVS đến VVF
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 34-44: V = VVS đến VVF
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11   // Luật 45-55: V = VVS đến VVF
    ];

    /// <summary>
    /// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
    /// 55 phần tử tương ứng với 55 luật
    /// Giá trị từ 1-11 tương ứng với 11 mức tốc độ đầu ra (0.0 - 3.0)
    /// 
    /// Logic bánh phải:
    /// - PI âm (NB, NM): Bánh phải chậm hơn → Robot rẽ trái
    /// - PI = 0 (Z): Bánh phải theo vận tốc V → Robot đi thẳng
    /// - PI dương (PB, PM): Bánh phải nhanh hơn → Robot rẽ phải
    /// </summary>
    private static readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT =
    [   
        // PI = NB (Negative Big) - Bánh phải RẤT CHẬM (rẽ trái mạnh)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             1,  1,  1,  2,  2,  3,  3,  4,  5,  6,  7,    // Luật 1-11
    
        // PI = Z (Zero) - Bánh phải THEO VẬN TỐC (đi thẳng)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             2,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11,   // Luật 12-22
    
        // PI = PB (Positive Big) - Bánh phải RẤT NHANH (rẽ phải mạnh)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             4,  5,  6,  7,  8,  9,  10, 10, 11, 11, 11,   // Luật 23-33
    
        // PI = NM (Negative Medium) - Bánh phải HƠI CHẬM (rẽ trái nhẹ)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             1,  2,  2,  3,  4,  5,  6,  7,  8,  9,  10,   // Luật 34-44
    
        // PI = PM (Positive Medium) - Bánh phải HƠI NHANH (rẽ phải nhẹ)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             2,  3,  4,  5,  6,  7,  8,  8,  9,  10, 11    // Luật 45-55
    ];

    /// <summary>
    /// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel)
    /// 110 phần tử: 55 cho PI + 55 cho Velocity
    /// </summary>
    private static readonly byte[] RULE_ANTECEDENT_INDICES_LEFT =
    [
        // 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,  // Luật 1-11:  PI = NB (tập 1)
        2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  // Luật 12-22: PI = Z  (tập 2)
        3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,  // Luật 23-33: PI = PB (tập 3)
        4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,  // Luật 34-44: PI = NM (tập 4)
        5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,  // Luật 45-55: PI = PM (tập 5)
    
        // 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 1-11
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 12-22
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 23-33
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,  // Luật 34-44
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11   // Luật 45-55
    ];

    /// <summary>
    /// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel)
    /// 55 phần tử tương ứng với 55 luật
    /// 
    /// Logic bánh trái: NGƯỢC LẠI với bánh phải
    /// - PI âm (NB, NM): Bánh trái nhanh hơn → Robot rẽ trái
    /// - PI = 0 (Z): Bánh trái theo vận tốc V → Robot đi thẳng
    /// - PI dương (PB, PM): Bánh trái chậm hơn → Robot rẽ phải
    /// </summary>
    private static readonly byte[] RULE_CONSEQUENT_INDICES_LEFT =
    [
        // PI = NB (Negative Big) - Bánh trái RẤT NHANH (rẽ trái mạnh)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             4,  5,  6,  7,  8,  9,  10, 10, 11, 11, 11,   // Luật 1-11
    
        // PI = Z (Zero) - Bánh trái THEO VẬN TỐC (đi thẳng)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             2,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11,   // Luật 12-22
    
        // PI = PB (Positive Big) - Bánh trái RẤT CHẬM (rẽ phải mạnh)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             1,  1,  1,  2,  2,  3,  3,  4,  5,  6,  7,    // Luật 23-33
    
        // PI = NM (Negative Medium) - Bánh trái HƠI NHANH (rẽ trái nhẹ)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             3,  4,  5,  6,  7,  8,  8,  9,  10, 11, 11,   // Luật 34-44
    
        // PI = PM (Positive Medium) - Bánh trái HƠI CHẬM (rẽ phải nhẹ)
        // V:   VVS VS  S-  S   S+  M   F-  F   F+  VF  VVF
             1,  2,  2,  3,  4,  4,  5,  6,  7,  8,  9,    // Luật 45-55
    ];

    /// <summary>
    /// Các mức đầu ra singleton (Output Singletons)
    /// 11 mức tốc độ từ 0.0 đến 3.0 m/s
    /// Độ phân giải: 3.0 / 10 = 0.3 m/s
    /// </summary>
    private static readonly double[] OUTPUT_SINGLETON_LEVELS =
    [
        0.0,   // Mức 1:  Dừng hoàn toàn (0%)
        0.3,   // Mức 2:  Rất chậm (10%)
        0.6,   // Mức 3:  Chậm (20%)
        0.9,   // Mức 4:  Hơi chậm (30%)
        1.2,   // Mức 5:  Chậm vừa (40%)
        1.5,   // Mức 6:  Trung bình (50%) - Điểm chuẩn
        1.8,   // Mức 7:  Hơi nhanh (60%)
        2.1,   // Mức 8:  Nhanh (70%)
        2.4,   // Mức 9:  Nhanh vừa (80%)
        2.7,   // Mức 10: Rất nhanh (90%)
        3.0    // Mức 11: Tối đa (100%)
    ];

    // ============================================================================
    // PHẦN 6: HÀM CHÍNH - BỘ ĐIỀU KHIỂN PI + FUZZY
    // ============================================================================

    // Biến trạng thái bộ tích phân
    private static double integratorState = 0.0;

    // Hệ số bộ điều khiển PI
    private static double proportionalGain = 1.0;  // Hệ số tỷ lệ (Kp)
    private static double integralGain = 0.1;      // Hệ số tích phân (Ki)

    /// <summary>
    /// Hàm điều khiển chính - Tính toán tốc độ bánh trái và phải
    /// Sử dụng bộ điều khiển PI kết hợp với logic mờ
    /// 
    /// CẤU HÌNH PHIÊN BẢN 3.0 - 11 TẬP MỜ:
    /// - PI Signal: 5 tập mờ (NB, Z, PB, NM, PM)
    /// - Velocity: 11 tập mờ (VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF)
    /// - Tổng số luật: 5 × 11 = 55 luật cho mỗi bánh
    /// - Dải đầu ra: 0.0 - 3.0 m/s
    /// - Độ phân giải: 0.3 m/s (tối ưu - cân bằng giữa độ mịn và hiệu suất)
    /// </summary>
    /// <param name="desiredVelocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
    /// <param name="desiredAngularVelocity">Vận tốc góc mong muốn (rad/s)</param>
    /// <param name="samplingTime">Chu kỳ lấy mẫu (giây, khuyến nghị 0.01s)</param>
    /// <returns>(leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 3.0]</returns>
    public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step(
        double desiredVelocity,
        double desiredAngularVelocity,
        double samplingTime)
    {
        const int NUM_INPUT_MEMBERSHIPS = 16;   // 5 cho PI + 11 cho Velocity
        const int NUM_OUTPUT_LEVELS = 11;       // 11 mức đầu ra (0.0 - 3.0)
        const int NUM_RULES = 55;               // 5 × 11 = 55 luật

        // Khởi tạo mảng lưu độ thuộc đầu vào
        double[] inputMembershipValues = new double[NUM_INPUT_MEMBERSHIPS];

        // Khởi tạo mảng lưu các mức đầu ra
        double[] outputLevels = new double[NUM_OUTPUT_LEVELS];
        Array.Copy(OUTPUT_SINGLETON_LEVELS, outputLevels, NUM_OUTPUT_LEVELS);

        // ========== BƯỚC 1: BỘ ĐIỀU KHIỂN PI ==========
        // Cập nhật trạng thái tích phân: I(t) = I(t-1) + Ki * error * dt
        integratorState += integralGain * desiredAngularVelocity * samplingTime;

        // Chống bão hòa tích phân (Anti-windup)
        integratorState = Math.Clamp(integratorState, -1.5, 1.5);

        // Tính tín hiệu điều khiển PI: u(t) = Kp * error + I(t)
        double piControlSignal = proportionalGain * desiredAngularVelocity + integratorState;

        // ========== BƯỚC 2: MỜ HÓA ĐẦU VÀO ==========
        FuzzifyPISignal(piControlSignal, inputMembershipValues);
        FuzzifyVelocity(desiredVelocity, inputMembershipValues);

        // ========== BƯỚC 3: TÍNH TOÁN BÁNH PHẢI ==========
        var (weightedSum_Right, totalWeight_Right) = EvaluateRules(
            inputMembershipValues,
            RULE_ANTECEDENT_INDICES_RIGHT,
            RULE_CONSEQUENT_INDICES_RIGHT,
            outputLevels,
            NUM_RULES
        );

        double rightWheelSpeed = Defuzzify(weightedSum_Right, totalWeight_Right, defaultValue: 1.5);

        // ========== BƯỚC 4: TÍNH TOÁN BÁNH TRÁI ==========
        var (weightedSum_Left, totalWeight_Left) = EvaluateRules(
            inputMembershipValues,
            RULE_ANTECEDENT_INDICES_LEFT,
            RULE_CONSEQUENT_INDICES_LEFT,
            outputLevels,
            NUM_RULES
        );

        double leftWheelSpeed = Defuzzify(weightedSum_Left, totalWeight_Left, defaultValue: 1.5);

        // ========== BƯỚC 5: GIỚI HẠN AN TOÀN ==========
        // Đảm bảo tốc độ không vượt quá giới hạn
        leftWheelSpeed = Math.Clamp(leftWheelSpeed, 0.0, 3.0);
        rightWheelSpeed = Math.Clamp(rightWheelSpeed, 0.0, 3.0);

        // ========== BƯỚC 6: TRẢ VỀ KẾT QUẢ ==========
        return (leftWheelSpeed, rightWheelSpeed);
    }

    // ============================================================================
    // PHẦN 7: HÀM HỖ TRỢ
    // ============================================================================

    /// <summary>
    /// Reset trạng thái bộ tích phân về 0
    /// Nên gọi khi bắt đầu chu kỳ điều khiển mới hoặc khi cần reset hệ thống
    /// </summary>
    public void ResetIntegrator()
    {
        integratorState = 0.0;
    }

    /// <summary>
    /// Thiết lập hệ số cho bộ điều khiển PI
    /// </summary>
    /// <param name="kp">Hệ số tỷ lệ (Proportional Gain) - Phản ứng với sai số hiện tại</param>
    /// <param name="ki">Hệ số tích phân (Integral Gain) - Loại bỏ sai số tích lũy</param>
    public FuzzyLogic WithPIGains(double kp, double ki)
    {
        proportionalGain = kp;
        integralGain = ki;
        return this;
    }

    /// <summary>
    /// Lấy trạng thái hiện tại của bộ tích phân
    /// Hữu ích cho việc debug và giám sát hệ thống
    /// </summary>
    /// <returns>Giá trị tích phân hiện tại</returns>
    public double GetIntegratorState()
    {
        return integratorState;
    }

    /// <summary>
    /// Lấy hệ số PI hiện tại
    /// </summary>
    /// <returns>(Kp, Ki): Hệ số tỷ lệ và tích phân</returns>
    public (double Kp, double Ki) GetPIGains()
    {
        return (proportionalGain, integralGain);
    }
}