module CORDIC_NCO
  #(
    parameter ACC_WIDTH = 32,
    parameter PHASE_WIDTH = 18,
    parameter OUTPUT_WIDTH = 16,
    parameter X0 = 19895
  )
  (
    input wire clk,
    input wire [ACC_WIDTH - 1: 0] phase_incr,
    output wire signed [OUTPUT_WIDTH - 1:0] Xout,
    output wire signed [OUTPUT_WIDTH - 1:0] Yout
  );

  localparam pi = 3.1415926535897932384626433832795;
  localparam WIDTH_XY = OUTPUT_WIDTH;
  localparam iterations = WIDTH_XY;

  reg signed [PHASE_WIDTH - 1:0] atan_table [iterations - 1:0];
  reg signed [PHASE_WIDTH - 1:0] z [iterations - 1:0];
  reg signed [WIDTH_XY - 1:0] x [iterations:0];
  reg signed [WIDTH_XY - 1:0] y [iterations:0];
  reg [ACC_WIDTH - 1:0] acc_phase = 0;

  wire signed [PHASE_WIDTH - 1:0] phase;

  integer i;

  assign phase = acc_phase[ACC_WIDTH - 1:ACC_WIDTH - PHASE_WIDTH];
  assign Xout = x[iterations][WIDTH_XY - 1:WIDTH_XY - OUTPUT_WIDTH];
  assign Yout = y[iterations][WIDTH_XY - 1:WIDTH_XY - OUTPUT_WIDTH];

  initial begin
    for (i = 0; i < iterations - 1; i = i + 1) begin
      atan_table[i] = ($atan(2.0 ** -i) / pi) * {1'b1, {PHASE_WIDTH - 1{1'b0}}};
    end
  end

  always @(posedge clk) begin
    acc_phase <= acc_phase + phase_incr;
  end

  always @(posedge clk) begin
    case (phase[PHASE_WIDTH - 1:PHASE_WIDTH - 2])
      2'b00, 2'b11: begin
        x[0] <= X0;
        y[0] <= 0;
        z[0] <= phase;
      end
      2'b01: begin
        x[0] <= 0;
        y[0] <= X0;
        z[0] <= {2'b00, phase[PHASE_WIDTH - 3:0]};
      end
      2'b10: begin
        x[0] <= 0;
        y[0] <= -X0;
        z[0] <= {2'b11, phase[PHASE_WIDTH - 3:0]};
      end
    endcase
    for (i = 0; i < iterations; i = i + 1) begin
      if (z[i] > 0) begin
        // * (2.0 ** -i)
        x[i + 1] <= x[i] - (y[i] >>> i);
        y[i + 1] <= y[i] + (x[i] >>> i);
        if (i < iterations - 1)
          z[i + 1] <= z[i] - atan_table[i];
      end else begin
        x[i + 1] <= x[i] + (y[i] >>> i);
        y[i + 1] <= y[i] - (x[i] >>> i);
        if (i < iterations - 1)
          z[i + 1] <= z[i] + atan_table[i];
      end
    end
  end
endmodule