/*
 * Spectrum and Network Analyzer
 * Copyright 2008 Mark Sandford
 * Based on code from Phil Covington and Darrell Harmon
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
   
// CORDIC Rotator
// 16 bit real input, 18 bit I/Q output, gain = 3.2934
// 20 bit phase input, scale: pi radians = 2^19
// SFDR appears to be > 112 dB
// Can be used for sin/cos or vector rotation
// Fully pipelined, latency of 15 cycles
module cordic_mjs(adc_in, iout, qout, ain, clk);
  input  [19:0] ain; // Angle 
  input  [15:0] adc_in;  // Real input to CORDIC rotator
  output [17:0] iout, qout; // I/Q output
  reg    [17:0] iout, qout;
  input         clk;
  reg    [18:0] Aa;
  reg    [17:0] Ab;
  reg    [17:0] Ax;
  reg    [16:0] Ad;
  reg    [15:0] Ae;
  reg    [14:0] Af;
  reg    [13:0] Ag;
  reg    [12:0] Ah;
  reg    [11:0] Ai;
  reg    [10:0] Aj;
  reg    [9:0]  Ak;
  reg    [8:0]  Al;
  reg    [7:0]  Am;
  reg    [6:0]  An;
  reg    [5:0]  Ao;
  reg    [4:0]  Ap;
  reg    [3:0]  Aq;
  reg    [19:0]     Ib, Ic, Id, Ie, If, Ig, Ih, Ii, Ij, Ik, Il, Im, In, Io, Ip, Iq;
  reg    [19:0] Qa, Qb, Qc, Qd, Qe, Qf, Qg, Qh, Qi, Qj, Qk, Ql, Qm, Qn, Qo, Qp, Qq;
  parameter stg1  = 77376; 
  parameter stg2  = 40883; 
  parameter stg3  = 20753; 
  parameter stg4  = 10416; 
  parameter stg5  = 5213 ; 
  parameter stg6  = 2607 ; 
  parameter stg7  = 1303 ; 
  parameter stg8  = 651  ; 
  parameter stg9  = 325  ; 
  parameter stg10 = 162  ; 
  parameter stg11 = 81   ; 
  parameter stg12 = 40   ; 
  parameter stg13 = 20   ; 
  parameter stg14 = 10   ; 
  parameter stg15 = 5    ; 
                                   
  always @ (posedge clk) begin
    // Prerotate: 90 degrees 
    Aa  <= {~ain[18],ain[17:0]};
    Qa <= ain[19] ? -{adc_in[15],adc_in,3'd4}: {adc_in[15],adc_in,3'd4};
      
    // 45 degrees 
    Ab[17:0] <= {~Aa[17],Aa[16:0]};
    Qb <= Qa;
    Ib <= Aa[18] ? Qa: -Qa;  

    Ax <= Ab[17] ? Ab + stg1: Ab - stg1;              
    Ic <= AddSub20( Ab[17], Ib, {{1{Qb[19]}}, Qb[19:1]});
    Qc <= AddSub20(~Ab[17], Qb, {{1{Ib[19]}}, Ib[19:1]});
    
    Ad <= Ax[17] ? Ax[16:0] + stg2: Ax[16:0] - stg2;             
    Id <= AddSub20( Ax[17], Ic, {{2{Qc[19]}}, Qc[19:2]});
    Qd <= AddSub20(~Ax[17], Qc, {{2{Ic[19]}}, Ic[19:2]});
    
    Ae <= Ad[16] ? Ad[15:0] + stg3: Ad[15:0] - stg3;             
    Ie <= AddSub20( Ad[16], Id, {{3{Qd[19]}}, Qd[19:3]});
    Qe <= AddSub20(~Ad[16], Qd, {{3{Id[19]}}, Id[19:3]});
     
    Af <= Ae[15] ? Ae[14:0] + stg4: Ae[14:0] - stg4;             
    If <= AddSub20( Ae[15], Ie, {{4{Qe[19]}}, Qe[19:4]});
    Qf <= AddSub20(~Ae[15], Qe, {{4{Ie[19]}}, Ie[19:4]});
     
    Ag <= Af[14] ? Af[13:0] + stg5: Af[13:0] - stg5;
    Ig <= AddSub20( Af[14], If, {{5{Qf[19]}}, Qf[19:5]});
    Qg <= AddSub20(~Af[14], Qf, {{5{If[19]}}, If[19:5]});
      
    Ah <= Ag[13] ? Ag[12:0] + stg6: Ag[12:0] - stg6;
    Ih <= AddSub20( Ag[13], Ig, {{6{Qg[19]}}, Qg[19:6]});
    Qh <= AddSub20(~Ag[13], Qg, {{6{Ig[19]}}, Ig[19:6]});
     
    Ai <= Ah[12] ? Ah[11:0] + stg7: Ah[11:0] - stg7;
    Ii <= AddSub20( Ah[12], Ih, {{7{Qh[19]}}, Qh[19:7]});
    Qi <= AddSub20(~Ah[12], Qh, {{7{Ih[19]}}, Ih[19:7]});
    
    Aj <= Ai[11] ? Ai[10:0] + stg8: Ai[10:0] - stg8;
    Ij <= AddSub20( Ai[11], Ii, {{8{Qi[19]}}, Qi[19:8]});
    Qj <= AddSub20(~Ai[11], Qi, {{8{Ii[19]}}, Ii[19:8]});
    
    Ak <= Aj[10] ? Aj[9:0] + stg9: Aj[9:0] - stg9;
    Ik <= AddSub20( Aj[10], Ij, {{9{Qj[19]}}, Qj[19:9]});
    Qk <= AddSub20(~Aj[10], Qj, {{9{Ij[19]}}, Ij[19:9]});
    
    Al <= Ak[9] ? Ak[8:0] + stg10: Ak[8:0] - stg10;
    Il <= AddSub20( Ak[9], Ik, {{10{Qk[19]}}, Qk[19:10]});
    Ql <= AddSub20(~Ak[9], Qk, {{10{Ik[19]}}, Ik[19:10]});
      
    Am <= Al[8] ? Al[7:0] + stg11: Al[7:0] - stg11;
    Im <= AddSub20( Al[8], Il, {{11{Ql[19]}}, Ql[19:11]});
    Qm <= AddSub20(~Al[8], Ql, {{11{Il[19]}}, Il[19:11]});
      
    An <= Am[7] ? Am[6:0] + stg12: Am[6:0] - stg12;
    In <= AddSub20( Am[7], Im, {{12{Qm[19]}}, Qm[19:12]});
    Qn <= AddSub20(~Am[7], Qm, {{12{Im[19]}}, Im[19:12]});
     
    Ao <= An[6] ? An[5:0] + stg13: An[5:0] - stg13;
    Io <= AddSub20( An[6], In, {{13{Qn[19]}}, Qn[19:13]});
    Qo <= AddSub20(~An[6], Qn, {{13{In[19]}}, In[19:13]});
          
    Ap <= Ao[5] ? Ao[4:0] + stg14: Ao[4:0] - stg14;
    Ip <= AddSub20( Ao[5], Io, {{14{Qo[19]}}, Qo[19:14]});
    Qp <= AddSub20(~Ao[5], Qo, {{14{Io[19]}}, Io[19:14]});
          
    Aq <= Ap[4] ? (Ap[3:0] + stg15):(Ap[3:0] - stg15); 
    Iq <= AddSub20( Ap[4], Ip, {{15{Qp[19]}}, Qp[19:15]});
    Qq <= AddSub20(~Ap[4], Qp, {{15{Ip[19]}}, Ip[19:15]});
    
    iout <= AddSub20( Aq[3], Iq[19:2], {{16{Qq[19]}}, Qq[19:18]});
    qout <= AddSub20(~Aq[3], Qq[19:2], {{16{Iq[19]}}, Iq[19:18]}); 
    end 
    
function [19:0] AddSub20;
  input Add;   
  input [19:0] Val;
  input [19:0] Mod;
  begin
    if (Add) AddSub20 = Val + Mod;
    else AddSub20 = Val - Mod;
    end
  endfunction
  
endmodule