% ////////////////////////////////////////////////////////////////////
% FILE,     HBF2LModel.m
% PROJECT,  HBF2L
% AUTHOR,   weitao.du
% DATE,     2007.Jun.11th
% VERSION,  0.1
% ////////////////////////////////////////////////////////////////////
% DECRIPTION
% 2 Level Half Band Fir Filter Design and Simulation, filer model 
% ////////////////////////////////////////////////////////////////////

function simData = HBF2LModel(cfg, simData);
  fprintf(1, 'HBF2LModel(), run\n');
  truncOut      = 0;
  dataFlp       = ModelFlp(cfg, simData, truncOut);
  dataFxpLv1    = ModelFxpLv1(cfg, simData, truncOut);

  simData.dataFlp       = dataFlp       ;
  simData.dataFxpLv1    = dataFxpLv1    ;

end  % function HBF2LModel();

% ////////////////////////////////////////////////////////////////////
% function ModelFxpLv1(), HBF2L fixed point model, 
% level 1, fixed point algorithm Model
function dataFxpLv1 = ModelFxpLv1(cfg, simData, truncOut);
% if truncOut ~= 0, then delete the for-and-aft FIR output data
% get config param
  inputFs   = cfg.inputFs      ;
  inputBw   = cfg.inputBw      ;
  inputWL   = cfg.inputWL      ;
  coeffWL   = cfg.coeffWL      ;
  outputWL  = cfg.outputWL     ;
  K1        = cfg.K1           ;
  K2        = cfg.K2           ;
  passBand1 = cfg.passBand1    ;
  passBand2 = cfg.passBand2    ;

  maxDataVal= (2^(inputWL-1)-1);    % inputWL include the sign bit

% get initial data
  filterInit   =  simData.filterInit   ;
  coeff1CdsFxp =  filterInit.coeff1CdsFxp;
  coeff2CdsFxp =  filterInit.coeff2CdsFxp;
  inputData    =  simData.inputData;

% HBF level 1 filter
  lenInput        =  length(inputData);
  lenOutLv1Part0  =  lenInput ;
  lenOutLv1Part1  =  lenOutLv1Part0;
% part0 gen by mult-add
  outLv1Part0     =  zeros(lenOutLv1Part0, 1);  
% part1 gen by copy
  outLv1Part1     =  zeros(lenOutLv1Part1, 1);  
% total output
  outLv1          =  zeros(lenOutLv1Part0*2,1);
% data sequence in the hbf, include the begining and end
% zero stuff
  lv1DataSeq      =  zeros(2*K1-1+lenInput, 1);
  lv1DataSeq(2*K1:lenInput+2*K1-1) = inputData;
% data sequence read for MAC index
  lv1DataIdx      =  [2*K1:lenInput+K1*2-1].';
% output index
  lv1OutIdx       =  [1:lenOutLv1Part0].';
% r1 is hbf level 1 sum index
  r1Idx           =  [0:K1-1];
  data0Idx0Trace   = zeros(lenOutLv1Part0,K1);
  data0Idx1Trace   = zeros(lenOutLv1Part0,K1);
  data1IdxTrace    = zeros(lenOutLv1Part1,1);

  for(idx1=1:lenOutLv1Part0)
  % level1 output part0, gen by add-mult-sum
    % the algorithm is explained in the design report
    data0Idx0  = lv1DataIdx(idx1)-r1Idx;
    data0Idx1  = lv1DataIdx(idx1)-2*K1+1 + r1Idx;
    dataPart0 = lv1DataSeq(data0Idx0);
    dataPart1 = lv1DataSeq(data0Idx1);
    dataAdd   = dataPart1+dataPart0;
    dataMult  = dataAdd .* coeff1CdsFxp;
    dataSum   = sum(dataMult);
    outLv1Part0(lv1OutIdx(idx1)) = dataSum; 
  % level1 output part1, gen by copy
    maxCoeffVal = 2^(coeffWL-1) - 1;    % coeffWL include the sign bit
    data1Idx  = lv1DataIdx(idx1)+1-K1;
    outLv1Part1(lv1OutIdx(idx1)) = lv1DataSeq(data1Idx) * maxCoeffVal;
    data0Idx0Trace(idx1,:)  = data0Idx0; 
    data0Idx1Trace(idx1,:)  = data0Idx1; 
    data1IdxTrace(idx1)     = data1Idx;
  end % for(idx1=1:lenOutLv1Part0)
  
  % normalize the output
  outLv1Part0 = floor(outLv1Part0/maxDataVal + 0.5);
  outLv1Part1 = floor(outLv1Part1/maxDataVal + 0.5);
  outLv1(1:2:2*lenOutLv1Part0-1) = outLv1Part0;
  outLv1(2:2:2*lenOutLv1Part0)   = outLv1Part1;
  if(truncOut ~= 0)
  % delete the pipe stuff data 
    outLv1(1:2*K1-1) = [];
  end
  cfftwinplot(outLv1, inputFs*2, 'HBF FIXED POINT LV1 output', 'green');

  % /////////////////////////////////////////////////////////////////////
  % HBF level 2 filter
  if(floor(K2/2) ~= K2/2) 
    fprintf(1,'WARNING, K2 is NOT a even integer, this will cause ERROR\n');
  end
  lenOutLv2Part0  =  lenInput;
  lenOutLv2Part1  =  lenOutLv2Part0;
  lenOutLv2Part2  =  lenOutLv2Part0;
  lenOutLv2Part3  =  lenOutLv2Part0;

  outLv2Part0     =  zeros(lenOutLv2Part0,1);
  outLv2Part1     =  zeros(lenOutLv2Part0,1);
  outLv2Part2     =  zeros(lenOutLv2Part0,1);
  outLv2Part3     =  zeros(lenOutLv2Part0,1);
  outLv2    = zeros(4*lenOutLv2Part0, 1);

  % x1 is the input data sequence for caculating the y2n
  x1              =  zeros(K1+K2-1 + lenInput, 1);
  x1(K1+K2 : lenInput + K1+K2-1) = inputData;
  % x1 read index
  x1Idx           =  [K1+K2 : lenInput + K1+K2-1];

  % zero padding y1_n0's start 
  y1_n0DataSeq    =  zeros(K2-1+lenOutLv1Part0, 1);
  y1_n0DataSeq(K2:lenOutLv1Part0+K2-1) = outLv1Part0;
  % y1_n0 read index
  y1_n0Idx        =  [K2 : lenOutLv1Part0 + K2-1]; 
  % level2 HBF output index, for part0~part3
  lv2OutIdx       =  [1:lenOutLv2Part0];
  % r2_Idx is the r2' in the design report
  r2_Idx          =  [0:K2/2 - 1];     
 
  y1EvenIdx_y2n0Trace = zeros(lenOutLv2Part0, K2/2); 
  x1EvenIdx_y2n0Trace = zeros(lenOutLv2Part0, K2/2); 
  y1OddIdx_y2n0Trace  = zeros(lenOutLv2Part0, K2/2); 
  x1OddIdx_y2n0Trace  = zeros(lenOutLv2Part0, K2/2); 
  y1EvenIdx_y2n2Trace = zeros(lenOutLv2Part0, K2/2); 
  x1EvenIdx_y2n2Trace = zeros(lenOutLv2Part0, K2/2); 
  y1OddIdx_y2n2Trace  = zeros(lenOutLv2Part0, K2/2); 
  x1OddIdx_y2n2Trace  = zeros(lenOutLv2Part0, K2/2); 

  for(idx1 = 1:lenOutLv2Part0) 
  % caculate part0
    y1EvenIdx_y2n0   = y1_n0Idx(idx1) - r2_Idx             ;
    x1EvenIdx_y2n0   = x1Idx(idx1)+ r2_Idx - K2  + 1 -K1   ;    
    y1OddIdx_y2n0    = y1_n0Idx(idx1) + r2_Idx - K2 + 1    ;    
    x1OddIdx_y2n0    = x1Idx(idx1) - r2_Idx -K1            ;
    % caculate the Even part of y2n0
    y2n0EvenAdd      = y1_n0DataSeq(y1EvenIdx_y2n0) + x1(x1EvenIdx_y2n0);
    y2n0EvenMult     = y2n0EvenAdd .* coeff2CdsFxp((2*r2_Idx + 1)); % +1 is for the MATLAB indexing(from 1)
    y2n0EvenSum      = sum(y2n0EvenMult);
    % caculate the Odd part of y2n0
    y2n0OddAdd       = y1_n0DataSeq(y1OddIdx_y2n0)  + x1(x1OddIdx_y2n0);
    y2n0OddMult      = y2n0OddAdd .* coeff2CdsFxp((2*r2_Idx + 1) + 1); % +1 is for the MATLAB indexing(from 1) 
    y2n0OddSum       = sum(y2n0OddMult);
    y2n0 = y2n0OddSum + y2n0EvenSum;
    
  % caculate part1
    y2n1Idx = x1Idx(idx1) - K2/2 - K1 + 1;
    y2n1    = x1(y2n1Idx) * maxCoeffVal;
    
  
  % caculate part2
    y1EvenIdx_y2n2   = y1_n0Idx(idx1) + r2_Idx - K2 + 1    ;
    x1EvenIdx_y2n2   = x1Idx(idx1) - r2_Idx + 1 - K1       ;
    y1OddIdx_y2n2    = y1_n0Idx(idx1) - r2_Idx             ;
    x1OddIdx_y2n2    = x1Idx(idx1) + r2_Idx - K2 + 2 -K1   ;    
    % caculate the Even part of y2n2
    y2n2EvenAdd      = y1_n0DataSeq(y1EvenIdx_y2n2) + x1(x1EvenIdx_y2n2);
    y2n2EvenMult     = y2n2EvenAdd .* coeff2CdsFxp((2*r2_Idx + 1)); % +1 is for the MATLAB indexing(from 1)
    y2n2EvenSum      = sum(y2n2EvenMult);
    % caculate the Odd part of y2n2
    y2n2OddAdd       = y1_n0DataSeq(y1OddIdx_y2n2) + x1(x1OddIdx_y2n2);
    y2n2OddMult      = y2n2OddAdd .* coeff2CdsFxp((2*r2_Idx + 1) + 1); % +1 is for the MATLAB indexing(from 1) 
    y2n2OddSum       = sum(y2n2OddMult);
    y2n2 = y2n2OddSum + y2n2EvenSum;
  
  % caculate part3
    y3n3Idx = y1_n0Idx(idx1) - K2/2 + 1;
    y2n3    = y1_n0DataSeq(y3n3Idx) * maxCoeffVal;

    outLv2Part0(idx1) = y2n0 ;
    outLv2Part1(idx1) = y2n1 ;
    outLv2Part2(idx1) = y2n2 ;
    outLv2Part3(idx1) = y2n3 ;

    y1EvenIdx_y2n0Trace(idx1,:) = y1EvenIdx_y2n0 ; 
    x1EvenIdx_y2n0Trace(idx1,:) = x1EvenIdx_y2n0 ; 
    y1OddIdx_y2n0Trace(idx1,:)  = y1OddIdx_y2n0  ;
    x1OddIdx_y2n0Trace(idx1,:)  = x1OddIdx_y2n0  ;

    y1EvenIdx_y2n2Trace(idx1,:) = y1EvenIdx_y2n2 ; 
    x1EvenIdx_y2n2Trace(idx1,:) = x1EvenIdx_y2n2 ; 
    y1OddIdx_y2n2Trace(idx1,:)  = y1OddIdx_y2n2  ;
    x1OddIdx_y2n2Trace(idx1,:)  = x1OddIdx_y2n2  ;


  end  % for(idx1 = 1:lenOutLv2Part0) 
  % normalize the output
  outLv2Part0 = floor (outLv2Part0 / maxDataVal + 0.5);
  outLv2Part1 = floor (outLv2Part1 / maxDataVal + 0.5);
  outLv2Part2 = floor (outLv2Part2 / maxDataVal + 0.5);
  outLv2Part3 = floor (outLv2Part3 / maxDataVal + 0.5);

  outLv2(1:4:4*lenOutLv2Part0-3) = outLv2Part0;
  outLv2(2:4:4*lenOutLv2Part0-2) = outLv2Part1;
  outLv2(3:4:4*lenOutLv2Part0-1) = outLv2Part2;
  outLv2(4:4:4*lenOutLv2Part0-0) = outLv2Part3;

  cfftwinplot(outLv2, inputFs*4, 'HBF FIXED POINT LV2 output', 'blue');

  % copy to output
  dbg.HBFLv1.outLv1Part0 = outLv1Part0;
  dbg.HBFLv1.outLv1Part1 = outLv1Part1;
  dbg.HBFLv1.lv1DataSeq  = lv1DataSeq;
  dbg.HBFLv1.lv1DataIdx  = lv1DataIdx;
  dbg.HBFLv1.lv1OutIdx   = lv1OutIdx;
  dbg.HBFLv1.data0Idx0Trace = data0Idx0Trace;
  dbg.HBFLv1.data0Idx1Trace = data0Idx1Trace;
  dbg.HBFLv1.data1IdxTrace  = data1IdxTrace;
  
  dbg.HBFLv2.y1_n0DataSeq    = y1_n0DataSeq;
  dbg.HBFLv2.outLv2Part0     = outLv2Part0;
  dbg.HBFLv2.outLv2Part1     = outLv2Part1;
  dbg.HBFLv2.outLv2Part2     = outLv2Part2;
  dbg.HBFLv2.outLv2Part3     = outLv2Part3;

  dbg.HBFLv2.y1EvenIdx_y2n0Trace  = y1EvenIdx_y2n0Trace;
  dbg.HBFLv2.x1EvenIdx_y2n0Trace  = x1EvenIdx_y2n0Trace;
  dbg.HBFLv2.y1OddIdx_y2n0Trace   = y1OddIdx_y2n0Trace;
  dbg.HBFLv2.x1OddIdx_y2n0Trace   = x1OddIdx_y2n0Trace;

  dbg.HBFLv2.y1EvenIdx_y2n2Trace  = y1EvenIdx_y2n2Trace;  
  dbg.HBFLv2.x1EvenIdx_y2n2Trace  = x1EvenIdx_y2n2Trace;  
  dbg.HBFLv2.y1OddIdx_y2n2Trace   = y1OddIdx_y2n2Trace;  
  dbg.HBFLv2.x1OddIdx_y2n2Trace   = x1OddIdx_y2n2Trace;  

  dataFxpLv1.outLv1 = outLv1;
  dataFxpLv1.outLv2 = outLv2;
  dataFxpLv1.dbg    = dbg;    

end % fucntion ModelFxpLv1()
% ////////////////////////////////////////////////////////////////////
% function ModelFlp(), HBF2L floating point model
function dataFlp = ModelFlp(cfg, simData, truncOut);
% if truncOut ~= 0, then delete the for-and-aft FIR output data

% get config param
  inputFs   = cfg.inputFs      ;
  inputBw   = cfg.inputBw      ;
  inputWL   = cfg.inputWL      ;
  coeffWL   = cfg.coeffWL      ;
  outputWL  = cfg.outputWL     ;
  K1        = cfg.K1           ;
  K2        = cfg.K2           ;
  passBand1 = cfg.passBand1    ;
  passBand2 = cfg.passBand2    ;

% get initial data
  filterInit   =  simData.filterInit   ;
  coeff1AllFxp =  filterInit.coeff1AllFxp ;
  coeff2AllFxp =  filterInit.coeff2AllFxp ;
  inputData    =  simData.inputData;

% HBF Level 1 input data interpolation and Filter
  lenIn1 = length(inputData); 
  dataLv1Itp = zeros(lenIn1*2, 1);
  dataLv1Itp(1:2:lenIn1*2-1) = inputData; 
  hbfLv1Out = conv(coeff1AllFxp, dataLv1Itp);
  
  if(truncOut ~= 0)
  % delete the pipe stuff data 
    hbfLv1Out(1:2*K1-1) = [];
    tmp = length(hbfLv1Out);
    hbfLv1Out(tmp-2*K1+2:tmp) = [];
  end
  cfftwinplot(hbfLv1Out, inputFs*2, 'HBF LV1 output', 'black');

% HBF Level 2 input data interpolation and Filter
  lenIn2 = length(hbfLv1Out); 
  dataLv2Itp = zeros(lenIn2*2, 1);
  dataLv2Itp(1:2:lenIn2*2-1) = hbfLv1Out; 
  hbfLv2Out = conv(coeff2AllFxp, dataLv2Itp);

  if(truncOut ~= 0)
    % delete the pipe stuff data 
    hbfLv2Out(1:2*K2-1) = [];
    tmp = length(hbfLv2Out);
    hbfLv2Out(tmp-2*K2+2:tmp) = [];
  end
  cfftwinplot(hbfLv2Out, inputFs*4, 'HBF LV2 output', 'black');
   
  dataFlp.hbfLv1Out     = hbfLv1Out;
  dataFlp.hbfLv2Out     = hbfLv2Out;
  dataFlp.dataLv1Itp    = dataLv1Itp;
end % function ModelFlp();
