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

        V. Smidl, Z. Peroutka

Rev. 28.10.2010   (ZP)

26.10.2010      Prvni verze (VS)

26.10.2010      Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA.
27.10.2010      Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci.
28.10.2010      Drobne upravy v kodu.

*************************************/

#include "matrix.h"
#include <math.h>

#define HIGH_PRECISION	0

/* 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; //in 15+qAU
    int *m2pom;
    int *m1pom=m1;
    int *respom=result;

    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)(*(m1pom++))**m2pom)>>(15-qAU);
                m2pom+=columns;
            }
            // add the missing A(i,j)
            tmp_sum += (long)(*m1pom)<<qAU; // no need to shift
            m1pom-=j; // shift back to first element

            *respom++=tmp_sum>>15;

            tmp_sum=0;
        }
        m1pom+=columns;
    }
};


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 is a row
        a_j=a;
        b_j=b;
        D_j=D;

        for (j=0; j<dimx; j++)
        {
            *b_j=((long)(*D_j)*(*a_j))>>15;
            b_j++;
            D_j++;
            a_j++;
        }

        alpha = R[iy]; //\alpha = R+vDv = R+a*b
        // R in q15, a in q15, b=q15

        a_j=a;
        b_j=b;
        D_j=D;

        for (j=0; j<dimx; j++)
        {
            lambda=alpha;
            if (HIGH_PRECISION == 0)
              alpha = alpha + (((long)(*a_j)*(*b_j))>>15);
            else
              alpha = (((long)alpha<<15) + (long)(*a_j)*(*b_j))>>15;	// xxxx lepsi presnost

            *D_j = ((long)lambda**D_j)/alpha;

            if (*D_j==0) *D_j=1;

            b_i=b;
            U_ij=U+j;

            for (i=0; i<j; i++)
            {
                beta   = *U_ij;
	            if (HIGH_PRECISION == 0)
				{
                  *U_ij = *U_ij - ((long)(*a_j)*(*b_i))/lambda;
                  *b_i  = *b_i + (((long)beta*(*b_j))>>15);
				}
				else
				{
                  *U_ij = ((long)lambda**U_ij - (long)(*a_j)*(*b_i))/lambda;
                  *b_i  = (((long)*b_i<<15) + (long)beta**b_j)>>15;					
				}
                b_i++;
                U_ij+=dimx;
            }

            a_j++;
            b_j++;
            D_j++;
        }

        x_i=xp;
        b_i=b;

        for (i=0; i<dimx; i++)
        {
          if (HIGH_PRECISION == 0)
            *x_i += ((long)difz[iy]**b_i)/alpha; // multiply by unscaled Kalman gain
          else  
            *x_i = ((long)alpha**x_i + (long)difz[iy]**b_i)/alpha; 

            x_i++;
            b_i++;
        }

        a+=dimx;
    }
}

/**/

// 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 qAU+15!!
    long z;
    long z_tmp;

    Dold_i=Dold;
    D_i=D;

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

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

    Dold_i=Dold+rows-1;
    D_i=D+rows-1;

    for (i=rows-1; 1; i--)
    { // stop if i==0 at the END!
        irows=i*rows;
        sigma = 0;

        PSIU_ik=PSIU+irows;
        Dold_k=Dold;

        for (k=0;k<rows; k++)
        {
            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
            PSIU_ik++;
            Dold_k++;
        }

	sigma += (long)*(Q+i+irows)<<(qAU+qD-15);

        G_ik=G+irows+i+1;

        for (j=i+1; j<rows; j++)
        {
            sigma += ((((long)(*G_ik)**G_ik)>>15)**(Q+j+j*rows))>>(30-qAU-qD);
            G_ik++;
        }

        if (sigma>((long)1<<(15+qAU)))
        {
	   	   *D_i = 32767;
		} else
        {
           *D_i=sigma>>qAU;
		}

        if (*D_i==0) *D_i=1;

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

            sigma =0;
            PSIU_ik=PSIU+irows;
            PSIU_jk=PSIU+jrows;
            Dold_k=Dold;

            for (k=0;k<rows; k++)
            {

                sigma += (((long)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k;
                PSIU_ik++;
                PSIU_jk++;
                Dold_k++;
            }

            G_ik=G+irows+i;
            G_jk=G+jrows+i;
            Q_kk=Q+i*rows+i;

            for (k=i;k<rows;k++)
            {
                sigma += ((((long)(*G_ik)**G_jk)>>15)**Q_kk)>>(30-qAU-qD);
                G_ik++;
                G_jk++;
                Q_kk+=rows+1;
            }

            z=(sigma/(*D_i))<<(15-qAU); // shift to q15
            if (z>32767) z=32767;
            if (z<-32768) z=-32768;

            U_ji=U+jrows+i;
            *U_ji = z;


            PSIU_ik=PSIU+irows;
            PSIU_jk=PSIU+jrows;

            for (k=0; k<rows;k++)
            {
              if (HIGH_PRECISION == 0)
                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
			  else
                *PSIU_jk = (((long)(*PSIU_jk)<<15) - (long)*U_ji**PSIU_ik)>>15;

                PSIU_ik++;
                PSIU_jk++;
            }

            G_jk=G+jrows;
            G_ik=G+irows;

            for (k=0;k<rows;k++)
            {
              if (HIGH_PRECISION == 0)
                *G_jk -= ((long)*U_ji**G_ik)>>15;
			  else
                *G_jk = (((long)(*G_jk)<<15) - (long)*U_ji**G_ik)>>15;
                 G_jk++;
                 G_ik++;
            }
        }

        Dold_i--;
        D_i--;
        if (i==0) return;
    }
}

/* square root of 0<a<1 using taylor at 0.5 in q15*/
int16 int_sqrt(int16 x) {
	double xd(double(x)/32768.);
	return (int)((sqrt(xd)+0.5)*32768);

    //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2
    //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5);
#define k1 23170 //0.5*sqrt(2)*32768
#define k2 11585 //0.25*sqrt(2)*32768

    int16 tmp;
    if (x>6554) {
        int16 xm05=x-16384;
        tmp = ((long)k1*xm05)>>15;
        tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
        tmp +=k1;
    } else {
        tmp = 4*x;
        tmp-=long(8*x)*x>>15;
    }
    return tmp;
}

void householder(int16 *Ch, int16 *Q, unsigned int16 dimx) 
{
    int16 k,j,i;
    int16 alpha,beta;
    int32  sigma; // 2*qCh
    int32  tmp_long;
    int16 B[25];//Q in qCh
    int16 w[5];
    int16 v[5];

    int16 *B_ij, *Q_i, *B_kj, *Ch_kj, *Ch_ij, *w_j, *v_j;

    B_ij=B;
    Q_i=Q;
    // copy Q to B
    for (i=0;i<dimx*dimx;i++)
    {
       *B_ij++=(*Q_i++)>>(15-qCh);
    }

    for (k=dimx-1; k>=0; k--)
    {
        sigma=0;
        B_kj=B+k*dimx+k;
        Ch_kj=Ch+k*dimx;

        for (j=k;j<dimx ;j++)
        {
            sigma+=((long)*B_kj**B_kj);
            B_kj++;
        }

        for (j=0;j<=k;j++)
        {
            sigma+=((long)*Ch_kj**Ch_kj);
            Ch_kj++;
        }

        //alpha in qCh
        alpha = (int)(sqrt((double)sigma)+0.5);   // verze pro PC
//        alpha = qsqrt(sigma);                     // verze pro DSP

	sigma=0;

        w_j=w;
        B_kj=B+k*dimx;

        for (j=0;j<dimx;j++)
        {
            *w_j=*B_kj;
            sigma+=(long)*w_j**w_j;
            w_j++;
            B_kj++;
        }

        v_j=v;
        Ch_kj=Ch+k*dimx;

        for (j=0; j<=k;j++)
        {
            if (j==k) {
                *v_j=*Ch_kj-alpha;
            } else {
                *v_j=*Ch_kj;
            }
            sigma+=(long)*v_j**v_j;

            v_j++;
            Ch_kj++;
        }

        alpha=sigma>>(qCh+1); // alpha = sigma /2;
        if (alpha==0) alpha =1;

        for (i=0;i<=k;i++)
        {
            sigma=0;

            B_ij=B+i*dimx+i;
            w_j=w+i;

            for (j=i;j<dimx;j++)
            {
                sigma+=((long)*B_ij**w_j);

                B_ij++;
                w_j++;
            }

            Ch_ij = Ch + i*dimx;
            v_j=v;

            for (j=0;j<=k;j++)
            {
                sigma+=(long)*Ch_ij**v_j;

                Ch_ij++;
                v_j++;
            }

            sigma = sigma >> 15;               // navrat do Q15
            if (sigma>32767) sigma=32767;


            B_ij=B+i*dimx+i;
            w_j=w+i;

            for (j=i;j<dimx;j++)
            {
		tmp_long=((long)*B_ij*alpha-sigma**w_j)/alpha;
		if (tmp_long>32767)
		   tmp_long=32767;
		if (tmp_long<-32768)
                   tmp_long=-32768;
		*B_ij++=tmp_long;
		w_j++;
            };

	    Ch_ij=Ch+i*dimx;
            v_j=v;

            for (j=0;j<=k;j++)
            {
                tmp_long=((long)*Ch_ij*alpha-sigma**v_j)/alpha;
		if (tmp_long>32767)
		   tmp_long=32767;
		if (tmp_long<-32768)
		   tmp_long=-32768;
		*Ch_ij++=tmp_long;
		v_j++;
            }
        }
    }
}


//  Nize uvedene fce jsou dle implementace v DSP
void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
    int16 alpha,beta,gamma;
    int16 delta, eta,epsilon,zeta,sigma,tau;
    int16 i,j,iy;
    int16 w[5];
	int32 tmp_long;

	int16 *Ch_ij, *w_i, *x_i;


    for (iy=0; iy<dimy; iy++)
    {
        alpha=R[iy];
        delta = difz[iy];

        for (j=0;j<dimx;j++)
        {
            sigma=Ch[iy*dimx+j];
            beta=alpha;
//            alpha+=((long)sigma*sigma)>>15;
            alpha=(((long)alpha<<15)+(long)sigma*sigma)>>15;				// vyssi presnost
//            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
            gamma= (int)(sqrt((double)((long)alpha*beta)));              // verze pro PC

            w[j]=0;

			Ch_ij=Ch+j;
			w_i=w;

            for (i=0;i<=j;i++)
            {
//                tau=Ch[i*dimx+j];
				tau=*Ch_ij;
//				tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
				tmp_long=((long)beta**Ch_ij -(long)sigma**w_i)/gamma;

				if (tmp_long>32767)
					tmp_long=32767;
				if (tmp_long<-32768)
					tmp_long=-32768;
				*Ch_ij=tmp_long;
			
//                w_i+=((long)tau*sigma)>>15;
                *w_i=(((long)*w_i<<15)+(long)tau*sigma)>>15;

				w_i++;
				Ch_ij+=dimx;
            }
        }

		x_i=xp;
		w_i=w;
        for (i=0;i<dimx;i++) {
//            xp[i]+=((long)w[i]*delta)/alpha;
//            *x_i+=((long)*w_i*delta)/alpha;
            *x_i=((long)*x_i*alpha+(long)*w_i*delta)/alpha;		// vyssi presnost
			x_i++;
			w_i++;
        }
    }
}

/* Matrix multiply Full matrix by upper diagonal matrix; */
void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
	unsigned int16 i, j, k;
	int32 tmp_sum=0L;
	int16 *m2pom;
	int16 *m1pom=m1;
	int16 *respom=result;

	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+=(int32)(*(m1pom++))**m2pom;
				m2pom+=columns;
			}
			m1pom-=(j+1); // shift back to first element

			*respom++=tmp_sum>>15;
			
			tmp_sum=0;
		}
		m1pom+=(columns);
	}
}

void givens(int16 *Ch, int16 *Q, unsigned int16 dimx)
{
	int16 i,j,k;
	int16 rho,s,c,tau;
	int32  tmp_long;

	int16 A[25];//beware

	int16 *A_ij, *Q_i, *Ch_ki, *Ch_kj, *Ch_ii, *Ch_ij, *A_kj;

	A_ij=A;
	Q_i=Q;
	// copy Q to A
	for (i=0;i<dimx*dimx;i++)
	{
//		A[i]=Q[i]>>(15-qCh);
	  *A_ij++=(*Q_i++)>>(15-qCh);
	}

	for (i=dimx-1; i>=0; i--)
	{
		Ch_ii=Ch+i*dimx+i;
		A_ij=A+i*dimx;

		for (j=0; j<dimx; j++) 
		{
//			tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j];

			tmp_long=(long)*Ch_ii**Ch_ii+(long)*A_ij**A_ij;

			if (tmp_long>0)
			{
//				rho=qsqrt(tmp_long);                   // verze pro DSP
				rho=(int)(sqrt((double)tmp_long));     // verze pro PC
				s=((long)*A_ij<<15)/rho;
				c=((long)*Ch_ii<<15)/rho;

				Ch_ki=Ch+i;
				A_kj=A+j;

				for (k=0;k<=i; k++)
				{
					tau=((long)c**A_kj-(long)s**Ch_ki)>>15;
					*Ch_ki=((long)s**A_kj+(long)c**Ch_ki)>>15;
					*A_kj=tau;

					Ch_ki+=dimx;
					A_kj+=dimx;
				}
			}
			A_ij++;
		}
	
	Ch_ij = Ch+i*dimx;

	for (j=0; j<i; j++)
	{
		tmp_long=(long)*Ch_ii**Ch_ii+(long)*Ch_ij**Ch_ij;
		
		if (tmp_long>0)
		{
//			rho=qsqrt(tmp_long);                     // verze pro DSP
			rho=(int)(sqrt((double)tmp_long));       // verze pro PC
			s=((long)*Ch_ij<<15)/rho;
			c=((long)*Ch_ii<<15)/rho;
			
			Ch_kj = Ch + j;
			Ch_ki = Ch + i;
			
			for (k=0; k<=i; k++)
			{
				tau=((long)c**Ch_kj-(long)s**Ch_ki)>>15;
				*Ch_ki =((long)s**Ch_kj+(long)c**Ch_ki)>>15;
				*Ch_kj=tau;

				Ch_kj += dimx;
				Ch_ki += dimx;
			}
		}
		Ch_ij++;
	}
  }
}