/************************************
        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);
    }
};

bool DBG=true;

void show(const char name[10], int *I, int n) {
	if (!DBG) return;
	
	printf("%s: ",name);
    for (int i=0;i<n;i++) {
		printf("%d ",*(I+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 q30!!!!!!
    for (i=rows-1; true;i--) { // check i==0 at the END!
        sigma = 0;
		
        for (j=0;j<rows; j++) {
			//long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
			long s2=(((long)PSIU[i*rows+j]*PSIU[i*rows+j])>>15)*(Dold[j]);
//			printf("%d - %d\n",s1,s2);
			sigma += s2;
		}
		sigma += Q[i*rows+i]<<15;
        for (j=i+1;j<rows; j++) {
            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[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;
//show("D",D,5);

        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;


            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;
            }
			
        }
        //show("U",U,25);
		//show("G",G,25);
		if (i==0) return;
    }
}

void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
    int alpha;
    int beta,lambda;
    int b[5]; // ok even for 4-dim state
    int *a; // in [0,1] -> q15
    unsigned int iy,j,i;

    int *b_j,*b_i;
    int *a_j;
    int *D_j;
    int *U_ij;
    int *x_i;
    a = U; // iyth row of U
    for (iy=0; iy<dimy; iy++, a+=dimx) {
        // a is a row
        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_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,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
            beta   = alpha;
            lambda = -((long)(*a_j)<<15)/beta;
            alpha  += ((long)(*a_j)*(*b_j))>>15;
            D[j] = ((((long)beta<<15)/alpha)*(*D_j))>>15; //gamma is long
            if (*D_j==0) *D_j=1;

            for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
                beta   = *U_ij;
                *U_ij +=  ((long)lambda*(*b_i))>>15;
                *b_i  +=  ((long)beta*(*b_j))>>15;
            }
        }
        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
        // no shift due to gamma
        for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain
        }

        //cout << "Ub: " << U << endl;
        //cout << "Db: " << D << endl <<endl;

    }

}

// Thorton procedure - Kalman predictive variance in UD
void thorton_fast(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_i,*Dold_k;
    int *D_i;
    int *PSIU_ij,*PSIU_ik,*PSIU_jk;
    int *Q_jj,*Q_ii,*Q_kk;
    int *U_ji;
    int *G_ik,*G_jk;
    int irows,jrows;
    long sigma; // in q30!!

    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
        *Dold_i=*D_i;
    }

    // initialize G = eye()
    G_ik= G;
    *G_ik++=1<<14;
    for (i=0;i<rows-1;i++) {
        // clean elem before diag
        for (k=0; k<rows; k++) {
            *G_ik++=0;
        }
        *G_ik++=1<<14;
    }
    // eye created

    for (i=rows-1, Dold_i=Dold+i, D_i=D+i;
            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
        irows=i*rows;
        sigma = 0;
        for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
           k<rows; k++, PSIU_ik++,Dold_k++) {//Dold_i=
            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_k);
		}
		sigma += *(Q+i+irows)<<15;
        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);
			
        }
		
        *D_i=sigma>>15;
        if (*D_i==0) *D_i=1;


        for (j=0;j<i;j++) {
            jrows = j*rows;

            sigma =0;
            for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold;
                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
				
                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
            }
			
            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
            k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
                sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk;
            }
			
			long z=sigma/(*D_i); // shift by 15
            if (z>32767) z=32767;
            if (z<-32768) z=-32768;

            U_ji=U+jrows+i;
            *U_ji = (int)z;


            for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows;
                    k<rows;k++,PSIU_ik++,PSIU_jk++) {
                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
            }
			
            for (k=0,G_jk=G+jrows,G_ik=G+irows;
            k<rows;k++, G_jk++, G_ik++) {
                *G_jk -=  ((long)*U_ji**G_ik)>>15;
            }
			
        }
        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]; // ok even for 4-dim state
    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;
            lambda = -((long)a[j]<<15)/beta;
            alpha  += ((long)(a[j])*b[j]>>15);
            D[j] = (((long)beta<<15)/alpha)*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 = (((long)difz[iy])<<15)/alpha;  // 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;

    }

}