% ////////////////////////////////////////////////////////////////////
% 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      = 1;
  dataFlp       = ModelFlp(cfg, simData, truncOut);
  dataFxp       = ModelFxp(cfg, simData);

  simData.dataFlp       = dataFlp       ;
  simData.dataFxp       = dataFxp       ;

end  % function HBF2LModel();

% ////////////////////////////////////////////////////////////////////
% function ModelFxp(), HBF2L fixed point model, 
function dataFxp = ModelFxp(cfg, simData);
% get config param
  inputFs   = cfg.inputFs      ;
  inputBw   = cfg.inputBw      ;

  inputWL   = cfg.inputWL      ;
  coeffWL   = cfg.coeffWL      ;
  adderWL   = cfg.adderWL      ;
  adderFL   = cfg.adderFL      ;
  multWL    = cfg.multWL       ;
  multFL    = cfg.multFL       ;
  outputWL  = cfg.outputWL     ;
  outputFL1 = cfg.outputFL1    ;
  outputFL2 = cfg.outputFL2    ;


  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   ;
  coeff1CdsFlt =  filterInit.coeff1CdsFlt;
  coeff2CdsFlt =  filterInit.coeff2CdsFlt;
  inputData    =  simData.inputData;

% fixed point config setting 
  fxpCfg = FxpSetting(inputWL, coeffWL, adderWL, adderFL, multWL, multFL, outputWL, outputFL1, outputFL2);
  % the output data will use the same config as input data 
  coeffNtype  =  fxpCfg.coeffNtype ;
  inputNtype  =  fxpCfg.inputNtype ;
  adderMath   =  fxpCfg.adderMath  ;
  multMath    =  fxpCfg.multMath   ;
  sumMathLv1  =  fxpCfg.sumMathLv1 ;
  sumMathLv2  =  fxpCfg.sumMathLv2 ;

  inputDataFxp = fi(inputData, inputNtype, 'roundMode', 'fix');
  coeff1CdsFxp = fi(coeff1CdsFlt, coeffNtype);
  coeff2CdsFxp = fi(coeff2CdsFlt, coeffNtype);
 
  cfftwinplot(double(inputData), inputFs*2, 'ModelFxp() inputData', 'GREEN');
  cfftwinplot(double(inputDataFxp), inputFs*2, 'ModelFxp() inputDataFxp', 'GREEN');


% HBF level 1 filter
  lenInput        =  length(inputData);
  lenOutLv1Part0  =  lenInput ;
  lenOutLv1Part1  =  lenOutLv1Part0;
% part0 gen by mult-add
  outLv1Part0     =  fi(zeros(lenOutLv1Part0, 1), inputNtype);  
% part1 gen by copy
  outLv1Part1     =  fi(zeros(lenOutLv1Part1, 1), inputNtype);  
% total output
  outLv1          =  fi(zeros(lenOutLv1Part0*2,1), inputNtype);
% data sequence in the hbf, include the begining and end
% zero stuff
if(1)
  lv1DataSeq      =  zeros(2*K1-1+lenInput, 1);
  lv1DataSeq(2*K1:lenInput+2*K1-1) = inputData;
else
  lv1DataSeq      =  fi(zeros(2*K1-1+lenInput, 1), inputNtype);
  lv1DataSeq(2*K1:lenInput+2*K1-1) = inputDataFxp;
end
  

% 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);
  if(1)
    dataAdd   = double(dataPart1) + double(dataPart0);
  else
    dataAdd   = adderMath.add(dataPart1, dataPart0);
  end

  if(1)
    dataMult  = double(dataAdd) .* double(coeff1CdsFlt);
  else    
    dataMult  = multMath.mpy(dataAdd, coeff1CdsFxp);
  end

  if(1)
    dataSum   = sum(double(dataMult));
  else
    dataSum   = FxpSum(dataMult, sumMathLv1);
  end

    outLv1Part0(lv1OutIdx(idx1)) = dataSum; 
  % level1 output part1, gen by copy
    data1Idx  = lv1DataIdx(idx1)+1-K1;
  if(1)
    outLv1Part1(lv1OutIdx(idx1)) = double(lv1DataSeq(data1Idx));
  else
    outLv1Part1(lv1OutIdx(idx1)) = lv1DataSeq(data1Idx);
  end

    data0Idx0Trace(idx1,:)  = data0Idx0; 
    data0Idx1Trace(idx1,:)  = data0Idx1; 
    data1IdxTrace(idx1)     = data1Idx;
  end % for(idx1=1:lenOutLv1Part0)
  
  % set the output
  outLv1(1:2:2*lenOutLv1Part0-1) = outLv1Part0;
  outLv1(2:2:2*lenOutLv1Part0)   = outLv1Part1;

  cfftwinplot(double(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) = double(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 .* coeff2CdsFlt((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 .* coeff2CdsFlt((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) ;
    
  
  % 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 .* coeff2CdsFlt((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 .* coeff2CdsFlt((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) ;

    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) 

  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;  

  dataFxp.outLv1 = outLv1;
  dataFxp.outLv2 = outLv2;
  dataFxp.dbg    = dbg;    

end % fucntion ModelFxpLv1()
% ////////////////////////////////////////////////////////////////////
% function FxpSetting(), fixed point property setting
function fxpCfg = FxpSetting(inputWL, coeffWL, adderWL, adderFL, multWL, multFL, outputWL, outputFL1, outputFL2);
  coeffNtype  = numerictype(1, coeffWL  , coeffWL  - 1 );
  inputNtype  = numerictype(1, inputWL  , inputWL  - 1 );
  
  adderMath  = fimath(...
    'CastBeforeSum'     ,      1                , ...
    'OverflowMode'      ,    'wrap'             , ...
    'SumMode'           ,    'SpecifyPrecision' , ...
    'SumWordLength'     ,    adderWL            , ...
    'SumFractionLength' ,    adderFL            , ...   
    'RoundMode'         ,    'round'            );

  multMath = fimath(...
    'ProductMode'               ,   'SpecifyPrecision'  , ...
    'ProductWordLength'         ,   multWL              , ...
    'ProductFractionLength'     ,   multFL              , ...   
    'RoundMode'                 ,   'round'             );
  sumMathLv1 = fimath(...
    'CastBeforeSum'     ,        1                      , ...
    'OverflowMode'      ,       'wrap'                  , ...
    'SumMode'           ,       'SpecifyPrecision'      , ...
    'SumWordLength'     ,       outputWL                , ...
    'SumFractionLength' ,       outputFL1               , ...   
    'RoundMode'         ,       'round'                 );
  sumMathLv2 = fimath(...
    'CastBeforeSum'     ,        1                      , ...
    'OverflowMode'      ,       'wrap'                  , ...
    'SumMode'           ,       'SpecifyPrecision'      , ...
    'SumWordLength'     ,       outputWL                , ...
    'SumFractionLength' ,       outputFL2               , ...   
    'RoundMode'         ,       'round'                 );


  fxpCfg.coeffNtype  = coeffNtype ;
  fxpCfg.inputNtype  = inputNtype ;
  fxpCfg.adderMath   = adderMath  ;
  fxpCfg.multMath    = multMath   ;
  fxpCfg.sumMathLv1  = sumMathLv1 ;
  fxpCfg.sumMathLv2  = sumMathLv2 ;

  
end % function FxpSetting() 
% ////////////////////////////////////////////////////////////////////
% function FxpSum(), fixed point sum 
% the input data and fixed point Math type should be correctly set
% this function will not check them

function sum = FxpSum(input, sumMath);
  len = length(input);
  sum = fi(0, input.numerictype);
  for(idx = 1:len)
    sum = sumMath.add(sum, input(idx));
  end  % for idx
end % function FxpSum(), fixed point sum 

% ////////////////////////////////////////////////////////////////////
% function ModelFlp(), HBF2L floating point model
function dataFlp = ModelFlp(cfg, simData, truncOut);
% if truncOut ~= 0, then delete the tail of 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 = hbfLv1Out(1 : 2*length(inputData));
  end
  cfftwinplot(hbfLv1Out, inputFs*2, 'FLOAT 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 = hbfLv2Out(1:4*length(inputData)); 
  end
  cfftwinplot(hbfLv2Out, inputFs*4, 'FLOAT HBF LV2 output', 'black');
   
  dataFlp.hbfLv1Out     = hbfLv1Out;
  dataFlp.hbfLv2Out     = hbfLv2Out;
  dataFlp.dataLv1Itp    = dataLv1Itp;
end % function ModelFlp();
