// 
//  Project: Experiment 7.6.3 ETSI normalized LMS filter - Chapter 7
//  File name: ETSI_nlms.   
//
//  Description: This is the normalized LMS algorithm using ETSI operators
//
//  For the book "Real Time Digital Signal Processing: 
//                Implementation and Application, 2nd Ed"
//                By Sen M. Kuo, Bob H. Lee, and Wenshun Tian
//                Publisher: John Wiley and Sons, Ltd
//
//  Tools used: CCS v.2.12.07
//              TMS320VC5510 DSK Rev-C
//

#include <gsm.h>             // GSM.h maps ETSI functions to TI intrinsics
#include "ETSI_nlms.h"

void intrinsic_nlms(LMS *lmsObj)
{ 
  LMS    *lms=(LMS *)lmsObj;
  long   temp32;
  short  j,n,mu,ue,*x,*w;
	
  n = lms->order;
  w = &lms->w[0];
  x = &lms->x[0];

  // Update data delay line
  for(j=n-1; j>0; j--)
  {
    x[j] = x[j-1];
  }
  x[0] = lms->in;
    
  // Compute normalized mu
  //temp32 = (((long)lms->x[0] * lms->x[0])+0x4000)>>15;  
  temp32 = mult_r(lms->x[0],lms->x[0]);  
		
  //temp32 = ((temp32 * ONE_MINUS_BETA)+0x4000)>>15;           
  temp32 = mult_r((short)temp32, ONE_MINUS_BETA);           
		
  //lms->power = (short)(((lms->power * (long)BETA)+0x4000)>>15);  
  lms->power = mult_r(lms->power, BETA);  
		
  //temp32 += (lms->power+lms->c);            
  temp32 = add(lms->power, (short)temp32);         
  temp32 = add(lms->c, (short)temp32); 
  temp32 >>= 1;                   
  mu = lms->alpha / (short)temp32;
    
  // Get adaptive filter output - equation (7.2.19)
  //temp32 = (long)w[0] * x[0];  
  temp32 = L_mult(w[0], x[0]); 	
  for(j=1; j<n; j++)
  {
    //temp32 += (long)w[j] * x[j];
    temp32 = L_mac(temp32, w[j], x[j]);        
  }
  //lms->out = (short)((temp32+ROUND)>>15);
  lms->out = round(temp32);
    
  // Compute error term - equation (7.2.20)
  //lms->err = lms->des - lms->out;
  lms->err = sub(lms->des, lms->out);
    
  // Coefficients update - equation (7.3.9)
  //ue = (long)(((lms->err * (long)mu)+ROUND)>>15); 
  ue = mult_r(lms->err, mu); 	
  for(j=0; j<n ; j++)
  {
    //w[j] += (short)((((long)ue * x[j])+ROUND)>>15);     
    w[j] = add(w[j], mult_r(ue, x[j]));        
  }
}
