namespace RobotNet10.RobotApp.Services.Navigation.CSharp;

public class FuzzyLogic
{
    private double Gain_P = 0.5;
    private double Gain_I = 0.01;
    private double piIntegratorState;  // Trạng thái tích phân của PI controller

    // Các tham số cho membership functions hình thang của tín hiệu góc
    // Negative Large: [-∞, -∞, -1.0, -0.5]
    private static readonly double[] NegativeLargeAngularParams = [-1.0E+10, -1.0E+10, -1.0, -0.5];
    // Positive Large: [0.5, 1.0, +∞, +∞]
    private static readonly double[] PositiveLargeAngularParams = [0.5, 1.0, 1.0E+10, 1.0E+10];

    // Các tham số cho membership functions hình thang của vận tốc
    // High Velocity: [0.75, 1.0, +∞, +∞]
    private static readonly double[] HighVelocityParams = [0.75, 1.0, 1.0E+9, 1.0E+9];
    // Low Velocity: [-∞, -∞, 0.0, 0.25]
    private static readonly double[] LowVelocityParams = [-1.0E+9, -1.0E+9, 0.0, 0.25];

    // Mảng quy tắc cho bộ điều khiển bánh phải (wr)
    // 25 phần tử đầu: chỉ số membership function cho tín hiệu góc (input 1)
    // 25 phần tử sau: chỉ số membership function cho vận tốc (input 2)
    private static readonly byte[] RightWheelRuleInput1Indices = [ 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 ];
    // Mảng quy tắc output cho bánh phải (25 quy tắc)
    private static readonly byte[] RightWheelRuleOutputIndices = [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];

    // Mảng quy tắc cho bộ điều khiển bánh trái (wl)
    private static readonly byte[] LeftWheelRuleInput1Indices = [  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 ];
    private static readonly byte[] LeftWheelRuleOutputIndices = [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];

    public void SetGainP(double gainP)
    {
        Gain_P = gainP;
    }

    public void SetGainI(double gainI)
    {
        Gain_I = gainI;
    }

    /// <summary>
    /// Tính toán giá trị membership cho hàm hình thang (trapezoidal membership function).
    /// </summary>
    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
    /// <param name="parameters">Mảng 4 phần tử: [a, b, c, d] trong đó:
    /// - a: điểm bắt đầu của cạnh tăng (left foot)
    /// - b: điểm bắt đầu của phần phẳng (left shoulder)
    /// - c: điểm kết thúc của phần phẳng (right shoulder)
    /// - d: điểm kết thúc của cạnh giảm (right foot)</param>
    /// <returns>Giá trị membership trong khoảng [0, 1]</returns>
    private static double Fuzzy_trapmf(double inputValue, double[] parameters)
    {
        // Extract các tham số để dễ đọc
        double leftFoot = parameters[0];      // a: điểm bắt đầu tăng
        double leftShoulder = parameters[1];  // b: điểm bắt đầu phẳng
        double rightShoulder = parameters[2]; // c: điểm kết thúc phẳng
        double rightFoot = parameters[3];     // d: điểm kết thúc giảm

        // Tính giá trị membership từ cạnh trái (từ a đến b)
        double leftMembership = 0.0;
        if (inputValue < leftFoot)
        {
            // Ngoài vùng hình thang bên trái
            leftMembership = 0.0;
        }
        else if (inputValue >= leftShoulder)
        {
            // Trong vùng phẳng bên trái
            leftMembership = 1.0;
        }
        else if (leftFoot != leftShoulder)
        {
            // Trên cạnh tăng (tính toán tuyến tính từ a đến b)
            leftMembership = (inputValue - leftFoot) / (leftShoulder - leftFoot);
        }

        // Tính giá trị membership từ cạnh phải (từ c đến d)
        double rightMembership = 0.0;
        if (inputValue <= rightShoulder)
        {
            // Trong vùng phẳng bên phải
            rightMembership = 1.0;
        }
        else if (inputValue > rightFoot)
        {
            // Ngoài vùng hình thang bên phải
            rightMembership = 0.0;
        }
        else if (rightShoulder != rightFoot)
        {
            // Trên cạnh giảm (tính toán tuyến tính từ c đến d)
            rightMembership = (rightFoot - inputValue) / (rightFoot - rightShoulder);
        }

        // Kết quả là giá trị nhỏ hơn để đảm bảo không vượt quá 1.0
        return leftMembership < rightMembership ? leftMembership : rightMembership;
    }

    /// <summary>
    /// Tính toán giá trị membership cho hàm tam giác (triangular membership function).
    /// </summary>
    /// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
    /// <param name="parameters">Mảng 3 phần tử: [a, b, c] trong đó:
    /// - a: điểm bắt đầu (left foot)
    /// - b: điểm đỉnh (peak) - giá trị membership = 1.0
    /// - c: điểm kết thúc (right foot)</param>
    /// <returns>Giá trị membership trong khoảng [0, 1]</returns>
    private static double Fuzzy_trimf(double inputValue, double[] parameters)
    {
        // Extract các tham số để dễ đọc
        double leftFoot = parameters[0];  // a: điểm bắt đầu
        double peak = parameters[1];       // b: điểm đỉnh
        double rightFoot = parameters[2];  // c: điểm kết thúc

        // Kiểm tra nếu giá trị nằm ngoài vùng tam giác
        if (inputValue < leftFoot || inputValue > rightFoot)
        {
            return 0.0;
        }

        // Nếu giá trị tại đỉnh, membership = 1.0
        if (inputValue == peak)
        {
            return 1.0;
        }

        // Tính toán membership trên cạnh tăng (từ a đến b)
        if (leftFoot < inputValue && inputValue < peak && leftFoot != peak)
        {
            return (inputValue - leftFoot) / (peak - leftFoot);
        }

        // Tính toán membership trên cạnh giảm (từ b đến c)
        if (peak < inputValue && inputValue < rightFoot && peak != rightFoot)
        {
            return (rightFoot - inputValue) / (rightFoot - peak);
        }

        // Trường hợp đặc biệt: nếu không khớp với điều kiện nào
        return 0.0;
    }

    /// <summary>
    /// Tính toán vận tốc bánh trái và bánh phải dựa trên fuzzy logic controller.
    /// </summary>
    /// <param name="v">Vận tốc tuyến tính (linear velocity) - giá trị chuẩn hóa [0, 1]</param>
    /// <param name="w">Vận tốc góc (angular velocity)</param>
    /// <param name="timeSample">Thời gian mẫu (sampling time) cho tích phân - phải > 0</param>
    /// <returns>Tuple chứa (wl: vận tốc bánh trái, wr: vận tốc bánh phải) - giá trị chuẩn hóa [0, 1]</returns>
    /// <exception cref="ArgumentException">Thrown khi timeSample <= 0 hoặc các tham số không hợp lệ</exception>
    public (double wl, double wr) Fuzzy_step(double v, double w, double timeSample)
    {
        // Validation đầu vào
        if (timeSample <= 0.0 || double.IsNaN(timeSample) || double.IsInfinity(timeSample))
        {
            throw new ArgumentException("timeSample must be a positive finite number", nameof(timeSample));
        }
        if (double.IsNaN(v) || double.IsNaN(w) || double.IsInfinity(v) || double.IsInfinity(w))
        {
            throw new ArgumentException("Input parameters v and w must be finite numbers", nameof(v));
        }
        
        (double wl, double wr) result = new();
        
        // Cache cho các giá trị membership của đầu vào (10 membership functions)
        // [0-4]: membership functions cho tín hiệu góc đã xử lý
        // [5-9]: membership functions cho vận tốc tuyến tính
        double[] inputMembershipValues = new double[10];
        
        // Cache cho các giá trị membership của đầu ra (5 levels: 0.0, 0.25, 0.5, 0.75, 1.0)
        double[] outputMembershipValuesRight = new double[5];  // Cho bánh phải (wr)
        double[] outputMembershipValuesLeft = new double[5];  // Cho bánh trái (wl)
        
        // Mảng tạm để chứa tham số cho hàm tam giác (3 phần tử: [a, b, c])
        double[] triangularParams = new double[3];
        
        // Các biến tạm để cache kết quả fuzzification (tránh tính toán lại)
        double negativeLargeMembership;
        double positiveLargeMembership;
        double highVelocityMembership;
        double lowVelocityMembership;
        // ========== BƯỚC 1: Xử lý tín hiệu đầu vào bằng PI Controller ==========
        // Tích phân vận tốc góc để loại bỏ sai số ổn định
        piIntegratorState += Gain_I * w * timeSample;
        // Kết hợp thành phần tỷ lệ và tích phân
        double piControllerOutput = Gain_P * w + piIntegratorState;
        
        // ========== BƯỚC 2: Fuzzification - Chuyển đổi đầu vào thành độ thuộc ==========
        // Tính toán membership values cho tín hiệu góc đã xử lý (5 membership functions)
        
        // MF1: Negative Large (hình thang)
        negativeLargeMembership = Fuzzy_trapmf(piControllerOutput, NegativeLargeAngularParams);
        inputMembershipValues[0] = negativeLargeMembership;
        
        // MF2: Negative (tam giác: -0.5, 0.0, 0.5)
        triangularParams[0] = -0.5;
        triangularParams[1] = 0.0;
        triangularParams[2] = 0.5;
        inputMembershipValues[1] = Fuzzy_trimf(piControllerOutput, triangularParams);
        
        // MF3: Positive Large (hình thang)
        positiveLargeMembership = Fuzzy_trapmf(piControllerOutput, PositiveLargeAngularParams);
        inputMembershipValues[2] = positiveLargeMembership;
        
        // MF4: Very Negative (tam giác: -1.0, -0.5, 0.0)
        triangularParams[0] = -1.0;
        triangularParams[1] = -0.5;
        triangularParams[2] = 0.0;
        inputMembershipValues[3] = Fuzzy_trimf(piControllerOutput, triangularParams);
        
        // MF5: Positive (tam giác: 0.0, 0.5, 1.0)
        triangularParams[0] = 0.0;
        triangularParams[1] = 0.5;
        triangularParams[2] = 1.0;
        inputMembershipValues[4] = Fuzzy_trimf(piControllerOutput, triangularParams);
        
        // Tính toán membership values cho vận tốc tuyến tính (5 membership functions)
        
        // MF6: Low (tam giác: 0.0, 0.25, 0.5)
        triangularParams[0] = 0.0;
        triangularParams[1] = 0.25;
        triangularParams[2] = 0.5;
        inputMembershipValues[5] = Fuzzy_trimf(v, triangularParams);
        
        // MF7: Medium (tam giác: 0.25, 0.5, 0.75)
        triangularParams[0] = 0.25;
        triangularParams[1] = 0.5;
        triangularParams[2] = 0.75;
        inputMembershipValues[6] = Fuzzy_trimf(v, triangularParams);
        
        // MF8: High (hình thang)
        highVelocityMembership = Fuzzy_trapmf(v, HighVelocityParams);
        inputMembershipValues[7] = highVelocityMembership;
        
        // MF9: Very Low (hình thang)
        lowVelocityMembership = Fuzzy_trapmf(v, LowVelocityParams);
        inputMembershipValues[8] = lowVelocityMembership;
        
        // MF10: High-Medium (tam giác: 0.5, 0.75, 1.0)
        triangularParams[0] = 0.5;
        triangularParams[1] = 0.75;
        triangularParams[2] = 1.0;
        inputMembershipValues[9] = Fuzzy_trimf(v, triangularParams);
        // ========== BƯỚC 3: Tính toán vận tốc bánh phải (wr) ==========
        // Khởi tạo giá trị membership cho đầu ra (5 mức: 0.0, 0.25, 0.5, 0.75, 1.0)
        outputMembershipValuesRight[0] = 0.0;
        outputMembershipValuesRight[1] = 0.25;
        outputMembershipValuesRight[2] = 0.5;
        outputMembershipValuesRight[3] = 0.75;
        outputMembershipValuesRight[4] = 1.0;
        
        // Đánh giá 25 quy tắc fuzzy và tính toán defuzzification
        double totalRuleActivation = 0.0;
        double weightedOutputSum = 0.0;
        const int numberOfRules = 25;
        
        for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
        {
            // Tính độ kích hoạt của quy tắc: product(input1_membership, input2_membership)
            // Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
            // input1: tín hiệu góc (index từ RightWheelRuleInput1Indices[ruleIndex] - 1, vì mảng bắt đầu từ 0)
            // input2: vận tốc (index từ RightWheelRuleInput1Indices[ruleIndex + 25] + 4, offset 4 vì vận tốc bắt đầu từ index 5)
            int angularSignalIndex = RightWheelRuleInput1Indices[ruleIndex] - 1;
            int velocityIndex = RightWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
            double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
            
            totalRuleActivation += ruleActivation;
            
            // Tính tổng có trọng số cho defuzzification (Center of Gravity)
            int outputIndex = RightWheelRuleOutputIndices[ruleIndex] - 1;
            weightedOutputSum += outputMembershipValuesRight[outputIndex] * ruleActivation;
        }
        
        // Defuzzification: Center of Gravity method
        if (totalRuleActivation == 0.0)
        {
            // Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
            result.wr = 0.5;
        }
        else
        {
            result.wr = weightedOutputSum / totalRuleActivation;
        }
        // ========== BƯỚC 4: Tính toán vận tốc bánh trái (wl) ==========
        // Sử dụng lại các giá trị membership đã tính ở BƯỚC 2 (không cần tính lại vì không thay đổi)
        // Các giá trị trong inputMembershipValues[0-9] đã được tính toán và lưu trữ ở BƯỚC 2
        
        // Khởi tạo giá trị membership cho đầu ra bánh trái
        outputMembershipValuesLeft[0] = 0.0;
        outputMembershipValuesLeft[1] = 0.25;
        outputMembershipValuesLeft[2] = 0.5;
        outputMembershipValuesLeft[3] = 0.75;
        outputMembershipValuesLeft[4] = 1.0;
        
        // Đánh giá 25 quy tắc fuzzy cho bánh trái và tính toán defuzzification
        totalRuleActivation = 0.0;
        weightedOutputSum = 0.0;
        
        for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
        {
            // Tính độ kích hoạt của quy tắc cho bánh trái: product(input1_membership, input2_membership)
            // Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
            // input1: tín hiệu góc (index từ LeftWheelRuleInput1Indices[ruleIndex] - 1)
            // input2: vận tốc (index từ LeftWheelRuleInput1Indices[ruleIndex + 25] + 4)
            int angularSignalIndex = LeftWheelRuleInput1Indices[ruleIndex] - 1;
            int velocityIndex = LeftWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
            double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
            
            totalRuleActivation += ruleActivation;
            
            // Tính tổng có trọng số cho defuzzification
            int outputIndex = LeftWheelRuleOutputIndices[ruleIndex] - 1;
            weightedOutputSum += outputMembershipValuesLeft[outputIndex] * ruleActivation;
        }
        
        // Defuzzification: Center of Gravity method cho bánh trái
        if (totalRuleActivation == 0.0)
        {
            // Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
            result.wl = 0.5;
        }
        else
        {
            result.wl = weightedOutputSum / totalRuleActivation;
        }
        return result;
    }
}