`ifndef FP_TRIG_CORDIC_SV
`define FP_TRIG_CORDIC_SV
`timescale 1ns / 1ps

// Synthesizable, combinational prototype: FP32 radians -> FP32 sin/cos/tan.
// Allowed |angle| <= 2*pi (FP32 encoding 0x40C90FDB).
// Internally uses signed 64-bit Q16.48 fixed point and 48 CORDIC stages.
// mode: 2'b01 = sin, 2'b10 = cos, 2'b11 = tan.
// Invalid angles (NaN, Inf, or outside supported range) yield quiet NaN.
module fp_trig_cordic (
    input  wire [31:0] angle_fp32,
    input  wire [1:0]  mode,
    output reg  [31:0] result
);
    localparam signed [63:0] PI_Q48      = 64'sd884279719003555;
    localparam signed [63:0] HALF_PI_Q48 = 64'sd442139859501778;
    localparam signed [63:0] TWO_PI_Q48  = 64'sd1768559438007110;
    localparam signed [63:0] CORDIC_K    = 64'sd170926505739102;
    localparam [31:0] QNAN             = 32'h7FC00000;
    localparam [30:0] MAX_ANGLE_BITS   = 31'h40C90FDB;
    localparam [30:0] SMALL_ANGLE_BITS = 31'h37800000; // 2^-16

    function automatic signed [63:0] fp32_to_q48(input [31:0] f);
        reg [23:0] significand;
        reg [63:0] magnitude;
        integer unbiased_exp, shift_amount;
        begin
            if (f[30:23] == 8'd0) begin
                unbiased_exp = -126;
                significand = {1'b0, f[22:0]};
            end else begin
                unbiased_exp = $unsigned(f[30:23]) - 127;
                significand = {1'b1, f[22:0]};
            end
            shift_amount = unbiased_exp + 25; // 48 fractional - 23 mantissa bits
            if (shift_amount >= 0)
                magnitude = {40'd0, significand} << shift_amount;
            else if (shift_amount <= -64)
                magnitude = 64'd0;
            else
                magnitude = {40'd0, significand} >> (-shift_amount);
            fp32_to_q48 = f[31] ? -$signed(magnitude) : $signed(magnitude);
        end
    endfunction

    function automatic signed [63:0] atan_q48(input integer index);
        begin
            case (index)
             0: atan_q48 = 64'sd221069929750889;
             1: atan_q48 = 64'sd130505199945453;
             2: atan_q48 = 64'sd68955363498242;
             3: atan_q48 = 64'sd35002819193903;
             4: atan_q48 = 64'sd17569333089919;
             5: atan_q48 = 64'sd8793231387230;
             6: atan_q48 = 64'sd4397688649582;
             7: atan_q48 = 64'sd2198978517948;
             8: atan_q48 = 64'sd1099506035422;
             9: atan_q48 = 64'sd549755114839;
            10: atan_q48 = 64'sd274877819563;
            11: atan_q48 = 64'sd137438942549;
            12: atan_q48 = 64'sd68719475371;
            13: atan_q48 = 64'sd34359738197;
            14: atan_q48 = 64'sd17179869163;
            15: atan_q48 = 64'sd8589934589;
            16: atan_q48 = 64'sd4294967296;
            17: atan_q48 = 64'sd2147483648;
            18: atan_q48 = 64'sd1073741824;
            19: atan_q48 = 64'sd536870912;
            20: atan_q48 = 64'sd268435456;
            21: atan_q48 = 64'sd134217728;
            22: atan_q48 = 64'sd67108864;
            23: atan_q48 = 64'sd33554432;
            24: atan_q48 = 64'sd16777216;
            25: atan_q48 = 64'sd8388608;
            26: atan_q48 = 64'sd4194304;
            27: atan_q48 = 64'sd2097152;
            28: atan_q48 = 64'sd1048576;
            29: atan_q48 = 64'sd524288;
            30: atan_q48 = 64'sd262144;
            31: atan_q48 = 64'sd131072;
            32: atan_q48 = 64'sd65536;
            33: atan_q48 = 64'sd32768;
            34: atan_q48 = 64'sd16384;
            35: atan_q48 = 64'sd8192;
            36: atan_q48 = 64'sd4096;
            37: atan_q48 = 64'sd2048;
            38: atan_q48 = 64'sd1024;
            39: atan_q48 = 64'sd512;
            40: atan_q48 = 64'sd256;
            41: atan_q48 = 64'sd128;
            42: atan_q48 = 64'sd64;
            43: atan_q48 = 64'sd32;
            44: atan_q48 = 64'sd16;
            45: atan_q48 = 64'sd8;
            46: atan_q48 = 64'sd4;
            47: atan_q48 = 64'sd2;
                default: atan_q48 = 64'sd0;
            endcase
        end
    endfunction

    // Round a signed Q16.48 value to binary32 (round-to-nearest, ties-to-even).
    function automatic [31:0] q48_to_fp32(input signed [63:0] value);
        reg neg, guard_bit, sticky_bit, round_up;
        reg [63:0] magnitude;
        reg [23:0] significand;
        reg [24:0] rounded;
        integer i, leading, shift_amount, exponent;
        begin
            if (value == 64'sd0) begin
                q48_to_fp32 = 32'h00000000;
            end else begin
                neg = value[63];
                magnitude = neg ? $unsigned(-value) : $unsigned(value);
                leading = 0;
                for (i = 0; i < 64; i = i + 1)
                    if (magnitude[i]) leading = i;
                exponent = leading - 48 + 127;
                shift_amount = leading - 23;
                guard_bit = 1'b0;
                sticky_bit = 1'b0;
                if (shift_amount > 0) begin
                    significand = magnitude >> shift_amount;
                    guard_bit = magnitude[shift_amount - 1];
                    if (shift_amount > 1)
                        sticky_bit = ((magnitude & ((64'd1 << (shift_amount - 1)) - 1)) != 0);
                end else begin
                    significand = magnitude << (-shift_amount);
                end
                round_up = guard_bit && (sticky_bit || significand[0]);
                rounded = {1'b0, significand} + {24'd0, round_up};
                if (rounded[24]) begin
                    rounded = rounded >> 1;
                    exponent = exponent + 1;
                end
                if (exponent >= 255)
                    q48_to_fp32 = {neg, 8'hFF, 23'd0};
                else if (exponent <= 0)
                    q48_to_fp32 = {neg, 31'd0}; // unreachable for nonzero Q16.48
                else
                    q48_to_fp32 = {neg, exponent[7:0], rounded[22:0]};
            end
        end
    endfunction

    // Round num/den to IEEE-754 binary32 without overflowing (num << 48).
    // Deliberately uses combinational 128-bit integer division; costly in area.
    function automatic [31:0] ratio_to_fp32(
        input signed [63:0] numerator,
        input signed [63:0] denominator
    );
        reg negative;
        reg [63:0] num_mag, den_mag;
        reg [127:0] dividend, divisor, quotient, remainder;
        reg [24:0] rounded;
        reg round_up;
        integer i, num_leading, den_leading, exp_unbiased, shift_amount, biased_exp;
        begin
            if (denominator == 0) begin
                ratio_to_fp32 = QNAN; // Q48 division singularity
            end else if (numerator == 0) begin
                ratio_to_fp32 = 32'h00000000;
            end else begin
                negative = numerator[63] ^ denominator[63];
                num_mag = numerator[63] ? $unsigned(-numerator) : $unsigned(numerator);
                den_mag = denominator[63] ? $unsigned(-denominator) : $unsigned(denominator);
                num_leading = 0;
                den_leading = 0;
                for (i = 0; i < 64; i = i + 1) begin
                    if (num_mag[i]) num_leading = i;
                    if (den_mag[i]) den_leading = i;
                end
                exp_unbiased = num_leading - den_leading;
                if (exp_unbiased >= 0) begin
                    if ({64'd0, num_mag} < ({64'd0, den_mag} << exp_unbiased))
                        exp_unbiased = exp_unbiased - 1;
                end else begin
                    if (({64'd0, num_mag} << (-exp_unbiased)) < {64'd0, den_mag})
                        exp_unbiased = exp_unbiased - 1;
                end
                // quotient has 24 significant bits (including implicit leading one).
                shift_amount = 23 - exp_unbiased;
                if (shift_amount >= 0) begin
                    dividend = {64'd0, num_mag} << shift_amount;
                    divisor  = {64'd0, den_mag};
                end else begin
                    dividend = {64'd0, num_mag};
                    divisor  = {64'd0, den_mag} << (-shift_amount);
                end
                quotient  = dividend / divisor;
                remainder = dividend % divisor;
                round_up = ((remainder << 1) > divisor) ||
                           (((remainder << 1) == divisor) && quotient[0]);
                rounded = {1'b0, quotient[23:0]} + {24'd0, round_up};
                if (rounded[24]) begin
                    rounded = rounded >> 1;
                    exp_unbiased = exp_unbiased + 1;
                end
                biased_exp = exp_unbiased + 127;
                if (biased_exp >= 255)
                    ratio_to_fp32 = {negative, 8'hFF, 23'd0};
                else if (biased_exp <= 0)
                    ratio_to_fp32 = {negative, 31'd0};
                else
                    ratio_to_fp32 = {negative, biased_exp[7:0], rounded[22:0]};
            end
        end
    endfunction

    reg signed [63:0] theta, x, y, z, next_x, next_y, sine_q, cosine_q;
    reg reverse_sign;
    integer stage;
    always @(*) begin
        theta = 64'sd0;
        x = 64'sd0;
        y = 64'sd0;
        z = 64'sd0;
        next_x = 64'sd0;
        next_y = 64'sd0;
        sine_q = 64'sd0;
        cosine_q = 64'sd0;
        reverse_sign = 1'b0;
        result = QNAN;

        if (angle_fp32[30:23] == 8'hFF || angle_fp32[30:0] > MAX_ANGLE_BITS) begin
            result = QNAN;
        end else if (angle_fp32[30:0] <= SMALL_ANGLE_BITS) begin
            // sin(x) ~= tan(x) ~= x and cos(x) ~= 1 for |x| <= 2^-16;
            // preserving signed zero and subnormal FP32 inputs.
            if (mode == 2'b10) result = 32'h3F800000;
            else result = angle_fp32;
        end else begin
            theta = fp32_to_q48(angle_fp32);
            // Reduce range to [-pi, pi] and then [-pi/2, pi/2].
            if (theta > PI_Q48) theta = theta - TWO_PI_Q48;
            else if (theta < -PI_Q48) theta = theta + TWO_PI_Q48;
            if (theta > HALF_PI_Q48) begin
                theta = theta - PI_Q48;
                reverse_sign = 1'b1;
            end else if (theta < -HALF_PI_Q48) begin
                theta = theta + PI_Q48;
                reverse_sign = 1'b1;
            end

            x = CORDIC_K;
            y = 64'sd0;
            z = theta;
            for (stage = 0; stage < 48; stage = stage + 1) begin
                if (z >= 0) begin
                    next_x = x - (y >>> stage);
                    next_y = y + (x >>> stage);
                    z = z - atan_q48(stage);
                end else begin
                    next_x = x + (y >>> stage);
                    next_y = y - (x >>> stage);
                    z = z + atan_q48(stage);
                end
                x = next_x;
                y = next_y;
            end
            sine_q = reverse_sign ? -y : y;
            cosine_q = reverse_sign ? -x : x;
            case (mode)
                2'b01: result = q48_to_fp32(sine_q);
                2'b10: result = q48_to_fp32(cosine_q);
                2'b11: result = ratio_to_fp32(sine_q, cosine_q);
                default: result = QNAN;
            endcase
        end
    end
endmodule
`endif