/************************************
        Extended Kalman Filter
        Matrix operations

        V. Smidl

Rev. 30.8.2010

30.8.2010      Prvni verze

*************************************/
#include "fixed.h"
#include "stdio.h"
/* Matrix multiply Full matrix by upper diagonal matrix; */
void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns){
  unsigned int i, j, k;
  long tmp_sum=0L;
  int *m2pom;

  for (i=0; i<rows; i++) //rows of result
  {
    for (j=0; j<columns; j++) //columns of result
    {
      m2pom=up+j;//??

      for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
      {
        tmp_sum+=(long)(*m1++)**m2pom;
        m2pom+=columns;
      }
      // add the missing A(i,j)
		tmp_sum +=(long)(*m1)<<15; // no need to shift
		m1-=(j); // shift back to first element

      // saturation effect
      tmp_sum=tmp_sum>>15;
      if (tmp_sum>32767) tmp_sum=32767;
      if (tmp_sum<-32768) tmp_sum=-32768;

      *result++=tmp_sum;

      tmp_sum=0;
    }
    m1+=(columns);
  }
};

void UDprt(int *U, int *D){
	return;
	for (int i=0;i<5;i++){
		for (int j=0;j<5;j++){
			printf("%d,",U[i*5+j]);
		}
		printf("\n");
	}
	for (int i=0;i<5;i++){printf("%d,",D[i]);}
	printf("\n");
}
 
// Thorton procedure - Kalman predictive variance in UD
void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows){
	unsigned int i,j,k;
	// copy D to Dold
	int *Dold_pom=Dold;
	for (i=0;i<rows;i++){*Dold_pom++=*D++;}
	D-=rows; // back to first D
	
	// initialize G = eye()
	int *G_pom = G;
	*G_pom++=1<<14;
	for (i=0;i<rows-1;i++){
		// clean elem before diag
		for (j=0; j<rows; j++){
			*G_pom++=0.0; 
		}
		*G_pom++=1<<14;
	}
	// eye created
	
	long sigma; // in q15
	for (i=rows-1; i>=0;i--){ // check i==0 at the END!
		sigma = 0;
		
		for (j=0;j<rows; j++){
			sigma += (((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
		}
		sigma += Q[i+i*rows];
		for (j=i+1;j<rows; j++){
			sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
		}
		
		*(D+i)=sigma>>15;
		if (D[i]==0) D[i]=1;

/*		printf("d=sig\n");
		UDprt(U,D);
		UDprt(G,Dold);*/
		
		for (j=0;j<i;j++){
// 			printf("\n%d,%d\n",i,j);
			sigma =0;
			for (k=0;k<rows;k++){
				sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k];
			}
			for (k=0;k<rows;k++){ 
				sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k]; 
			}		
			long z=sigma/D[i]; // shift by 15
			if (z>32767) z=32767;
			if (z<-32768) z=-32768;  
			
			U[j*rows+i] = (int)z;

/*			printf("U=sig/D\n");
			UDprt(U,D);
			UDprt(G,Dold);*/
			
			for (k=0;k<rows;k++){ 
				PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; 
			}
			for (k=0;k<rows;k++){ 
				G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15; 
			}
			
/*			printf("end\n");
			UDprt(U,D);
			UDprt(G,Dold);
			printf("\n");	*/
		}
		if(i==0) return;
	}
}

void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ){
	long alpha;
	long gamma,beta,lambda;
	int b[5];
	int *a; // in [0,1] -> q15
	unsigned int iy,j,i;
	
	for (iy=0; iy<dimy; iy++){
		// a is a row 
		a = U+iy*dimx; // iyth row of U
		for (j=0;j<dimx;j++)
			b[j]=((long)D[j]*a[j])>>15;
		
		alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
		// R in q15, a in q15, b=q15
		gamma = (1<<15)/alpha; //(in q15)
		//min alpha = R[iy] = 164
		//max gamma = 0.0061 => gamma_ref = q7
		for (j=0;j<dimx;j++){
			beta   = alpha; 
			alpha  += ((long)(a[j])*b[j]>>15); 
			lambda = -(a[j])*gamma>>15; 
			gamma  = (1<<15)/alpha; // in q15 now
			int mpl=((long)beta*gamma); // no-shift, max_gamma=2^15
			D[j] = (long)mpl*D[j]>>15; //gamma is long
			if (D[j]==0) D[j]=1;
			
			// 			cout << "a: " << alpha << "g: " << gamma << endl;
			for (i=0;i<j;i++){
				beta   = U[i*dimx+j]; 
				U[i*dimx+j] +=  (lambda*b[i])>>15; 
				b[i]   +=  (beta*b[j])>>15; 
			}
		}
		int dzs = (gamma*(difz[iy]))>>15;  // apply scaling to innovations 
									// no shift due to gamma
		for (i=0; i<dimx; i++){
			xp[i]  += ((long)dzs*b[i])>>15; // multiply by unscaled Kalman gain 
		}
		
		//cout << "Ub: " << U << endl;
		//cout << "Db: " << D << endl <<endl;
		
	}
	
}