`timescale 1ns/1ps

module fp_sincos_cordic (
    input  wire [31:0] angle_fp32,
    input  wire        cosine,        // 0 = sine, 1 = cosine
    output reg  [31:0] result
);
    localparam signed [31:0] PI      = 32'sd843314857;
    localparam signed [31:0] HALF_PI = 32'sd421657428;
    localparam signed [31:0] TWO_PI  = 32'sd1686629713;
    localparam signed [31:0] K       = 32'sd163008219;
    localparam [30:0] MAX_ANGLE     = 31'h40C90FDB; // 2*pi rounded to FP32
    localparam [30:0] SMALL_ANGLE   = 31'h35800000; // 2^-20

 
    function automatic signed [31:0] fp_to_q28(input [31:0] f);
        reg [23:0] mant;
        reg [31:0] mag;
        integer exp, sh;
        begin
            exp  = $unsigned(f[30:23]) - 127;
            mant = {1'b1, f[22:0]};
            sh   = exp + 5; // +28 fixed fractional -23 mantissa bits
            if (sh >= 0)       mag = {8'b0, mant} << sh;
            else if (sh <= -32) mag = 32'd0;
            else               mag = {8'b0, mant} >> (-sh);
            fp_to_q28 = f[31] ? -$signed(mag) : $signed(mag);
        end
    endfunction

    function automatic signed [31:0] atan_q28(input integer i);
        begin
            case (i)
                0: atan_q28=32'sd210828714;
                1: atan_q28=32'sd124459457;
                2: atan_q28=32'sd65760959;
                3: atan_q28=32'sd33381290;
                4: atan_q28=32'sd16755422;
                5: atan_q28=32'sd8385879;
                6: atan_q28=32'sd4193963;
                7: atan_q28=32'sd2097109;
                8: atan_q28=32'sd1048571;
                9: atan_q28=32'sd524287;
               10: atan_q28=32'sd262144;
               11: atan_q28=32'sd131072;
               12: atan_q28=32'sd65536;
               13: atan_q28=32'sd32768;
               14: atan_q28=32'sd16384;
               15: atan_q28=32'sd8192;
               16: atan_q28=32'sd4096;
               17: atan_q28=32'sd2048;
               18: atan_q28=32'sd1024;
               19: atan_q28=32'sd512;
               20: atan_q28=32'sd256;
               21: atan_q28=32'sd128;
               22: atan_q28=32'sd64;
               23: atan_q28=32'sd32;
               default: atan_q28=32'sd0;
            endcase
        end
    endfunction


    function automatic [31:0] q28_to_fp(input signed [31:0] q);
        reg sign, guard, sticky, up;
        reg [31:0] mag;
        reg [23:0] mant;
        reg [24:0] rounded;
        integer i, lead, sh, exp;
        begin
            sign = q[31];
            mag = sign ? $unsigned(-q) : $unsigned(q);
            lead = -1;
            for (i = 0; i < 32; i = i+1)
                if (mag[i]) lead = i;
            if (lead < 0) begin
                q28_to_fp = {sign, 31'd0};
            end else begin
                exp = lead - 28 + 127;
                sh = lead - 23;
                guard = 0; sticky = 0;
                if (sh > 0) begin
                    mant = mag >> sh;
                    guard = mag[sh-1];
                    if (sh > 1)
                        sticky = ((mag & ((32'd1 << (sh-1)) - 1)) != 0);
                end else
                    mant = mag << (-sh);
                up = guard && (sticky || mant[0]);
                rounded = {1'b0, mant} + {24'b0, up};
                if (rounded[24]) begin
                    rounded = rounded >> 1;
                    exp = exp + 1;
                end
                if (exp >= 255) q28_to_fp = {sign,8'hFF,23'd0};
                else if (exp <= 0) q28_to_fp = {sign,31'd0};
                else q28_to_fp = {sign,exp[7:0],rounded[22:0]};
            end
        end
    endfunction

    reg signed [31:0] theta, x, y, z, nx, ny, answer_q;
    reg flip;
    integer i;
    always @(*) begin
        theta=0; x=0; y=0; z=0; nx=0; ny=0; answer_q=0;
        flip=0; result=32'h7FC00000;
        if (angle_fp32[30:23] == 8'hFF || angle_fp32[30:0] > MAX_ANGLE) begin
            result = 32'h7FC00000; 
        end else if (angle_fp32[30:0] <= SMALL_ANGLE) begin
            result = cosine ? 32'h3F800000 : angle_fp32;
        end else begin
            theta = fp_to_q28(angle_fp32);
            if (theta > PI) theta = theta - TWO_PI;
            else if (theta < -PI) theta = theta + TWO_PI;
            if (theta > HALF_PI) begin
                theta = theta - PI;
                flip = 1;
            end else if (theta < -HALF_PI) begin
                theta = theta + PI;
                flip = 1;
            end

            x = K; y = 0; z = theta;
            for (i = 0; i < 24; i = i+1) begin
                if (z >= 0) begin
                    nx = x - (y >>> i);
                    ny = y + (x >>> i);
                    z = z - atan_q28(i);
                end else begin
                    nx = x + (y >>> i);
                    ny = y - (x >>> i);
                    z = z + atan_q28(i);
                end
                x = nx; y = ny;
            end
            answer_q = cosine ? x : y;
            if (flip) answer_q = -answer_q;
            result = q28_to_fp(answer_q);
        end
    end
endmodule