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

        Z. Peroutka

Rev. 15.3.2008

15.3. 2008      Kompletni kontrola vypoctu + zamena q15->int a q30->long

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

#include "matrix.h"

/* Vsechny meze se udavaji ve tvaru (rozmer_matice - 1), abych mel ve FOR konstantni horni mez) */

/* Matrix addition in q15: m1 + m2 = result[rows, columns] */
void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);

/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns);

/* Matrix substraction in q15: m1 - m2 = result[rows, columns] */
void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns);

/* Matrix multiply in q15: m1[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] */
void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns);

/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columnsx,columns]transpose) = result[rows,columns] */
void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns);

/* matrix multiplication in q15: sum is in q15 */
void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns);


/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns);


/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns);

/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
minrowcol=minimum(rows, columnsx) */
void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol);

/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns]
minrowcol=minimum(rows, columnsx) */
void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol);

/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
minrowcol=minimum(columnsx,columns) */
void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol);

/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns]
minrowcol=minimum(columnsx,columns) */
void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol);

/* Matrix transposition in q15: m1.' = result[rows, columns] */
void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns);

/* Matrix [2,2] inversion in q15: inv(m1) = result[rows, columns] */
void minv2(int *matrix, long *result);

void choice_P(int *m, int *result, unsigned int columns);
void choice_x(int *m, int *result);


/* matrix addition in q15 */
void madd(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns)
{
  unsigned int i,j;

  for (i=0; i<=rows; i++)
    for (j=0; j<=columns; j++)
      *result++ = *m1++ + *m2++;
}


/* add diagonal matrix m2 to matrix m1 - both in format q15, minrowcol = min(rows, columns) */
void maddD(int *m1, int *m2, unsigned int minrowcol, unsigned int columns)
{
  unsigned int i;

  for (i=0; i<=minrowcol; i++)
/*    *(m1+i*(columns+1)+i) += *(m2+i*(columns+1)+i); */
  {
    *m1 += *m2;
    m1+=(columns+2);
    m2+=(columns+2);
  }

}

/* Matrix substraction in q15 */
void msub(int *m1, int *m2, int *result, unsigned int rows, unsigned int columns)
{
  unsigned int i,j;

  for (i=0; i<=rows; i++)
    for (j=0; j<=columns; j++)
      *result++ = *m1++ - *m2++;
}


/* matrix multiplication in q15 */
void mmult(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns)
{
  unsigned int i, j, k;
  long tmp_sum=0;
  int *m2pom;

  for (i=0; i<=rows; i++)
  {
    for (j=0; j<=columns; j++)
    {
      m2pom=m2+j;

      for (k=0; k<=columnsx; k++)
      {
        tmp_sum+=(long)(*m1++)**m2pom;
        m2pom+=columns+1;
      }

      // 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-=(columnsx+1);
    }
    m1+=(columnsx+1);
  }
}


/* Matrix multiplication in q15: m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
void mmultt(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns)
{
  unsigned int i, j, k;
  long tmp_sum=0;
  int *m2pom=m2;

  for (i=0; i<=rows; i++)
  {
    for (j=0; j<=columns; j++)
    {
      for (k=0; k<=columnsx; k++)
        tmp_sum+=(long)(*m1++)*(*m2pom++);

      // 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-=(columnsx+1);
    }
    m2pom=m2;
    m1+=(columnsx+1);
  }
}


/* matrix multiplication in q15: sum is in q15 */
void mmult15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns)
{
  unsigned int i, j, k;
  long tmp_sum=0;
  int *m2pom;

  for (i=0; i<=rows; i++)
  {
    for (j=0; j<=columns; j++)
    {
      m2pom=m2+j;

      for (k=0; k<=columnsx; k++)
      {
        tmp_sum+=((long)(*m1++)**m2pom)>>15;
        m2pom+=columns+1;
      }

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

      *result++=tmp_sum;

      tmp_sum=0;
      m1-=(columnsx+1);
    }
    m1+=(columnsx+1);
  }
}


/* Matrix multiplication in q15 (sum is in q15): m1[rows,columnsx]*(m2[columns,columnsx]transpose) = result[rows,columns] */
void mmultt15(int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns)
{
  unsigned int i, j, k;
  long tmp_sum=0;
  int *m2pom=m2;

  for (i=0; i<=rows; i++)
  {
    for (j=0; j<=columns; j++)
    {
      for (k=0; k<=columnsx; k++)
        tmp_sum+=((long)(*m1++)*(*m2pom++))>>15;

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

      *result++=tmp_sum;

      tmp_sum=0;
      m1-=(columnsx+1);
    }
    m2pom=m2;
    m1+=(columnsx+1);
  }
}


/* Matrix multiplication - Q15 * Q30 format -> RESULT in Q15 */
void mmult1530(int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns)
{
  unsigned int i, j, k;
  long tmp_sum=0;
  long *m2pom;

  for (i=0; i<=rows; i++)
  {
    for (j=0; j<=columns; j++)
    {
      m2pom=m2+j;

      for (k=0; k<=columnsx; k++)
      {
        tmp_sum+=(long)(*m1++)**m2pom;
        m2pom+=columns+1;
      }

      // 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-=(columnsx+1);
    }
    m1+=(columnsx+1);
  }
}
 

/* Left matrix multiplication with DIAG ones matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] 
minrowcol=minimum(rows, columnsx) */
void mmultDl(int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol)
{
  unsigned int i,j;
/*  int *res_tmp=result; */
  
/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */
/*  for (i=0;i<=rows;i++)	
    for (j=0;j<=columns;j++)
      *res_tmp++=0;
/* END OF ERASE SEQUENCE */

  for (i=0;i<=minrowcol;i++)
    for (j=0;j<=columns;j++)
      *result++=*m2++;
}


/* Left matrix multiplication with DIAG matrix in q15: DIAG[rows,columnsx]*m2[columnsx,columns] = result[rows,columns] 
minrowcol=minimum(rows, columnsx) */
void mmultDl15(int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol)
{
  unsigned int i,j;
/*  int *res_tmp=result; */

/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */  
/*  for (i=0;i<=rows;i++)		
    for (j=0;j<=columns;j++)
      *res_tmp++=0;
/* END OF ERASE SEQUENCE */

  for (i=0;i<=minrowcol;i++)
  {
    for (j=0;j<=columns;j++)
      *result++=((long)(*DIAG)*(*m2++))>>15;
    
    DIAG+=(columnsx+1);
  }
}


/* Right matrix multiplication with DIAG ones matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns] 
minrowcol=minimum(columnsx,columns) */
void mmultDr(int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol)
{
  unsigned int i,j;
/*  int *res_tmp=result; */

/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */
/*  for (i=0;i<=rows;i++)
    for (j=0;j<=columns;j++)
      *res_tmp++=0;
/* END OF ERASE SEQUENCE */

  for (i=0;i<=rows;i++)
  {
    for (j=0;j<=minrowcol;j++)
      *result++=*m1++;
    
    m1+=(columnsx-minrowcol);
    result+=(columns-minrowcol);
  }
}  



/* Right matrix multiplication with DIAG matrix in q15: m1[rows,columnsx]*DIAG[columnsx,columns] = result[rows,columns] 
minrowcol=minimum(columnsx,columns) */
void mmultDr15(int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int
columns, unsigned int minrowcol)
{
  unsigned int i,j;
  int *DIAG_tmp=DIAG;
/*  int *res_tmp=result; */

/* This erase sequence have to be enabled in case that result matrix has more terms than DIAG matrix */  
/*  for (i=0;i<=rows;i++)		
    for (j=0;j<=columns;j++)
      *res_tmp++=0;
/* END OF ERASE SEQUENCE */

  for (i=0;i<=rows;i++)
  {
    for (j=0;j<=minrowcol;j++)
    {
      *result++=((long)(*m1++)**DIAG_tmp)>>15;
      DIAG_tmp+=columns+2;
    }
     
    DIAG_tmp=DIAG;
    m1+=(columnsx-minrowcol);
    result+=(columns-minrowcol);
  }
}  


/* matrix transposition - neni optimalizovana, protoze se nepouziva */
void mtrans(int *m1, int *result, unsigned int rows, unsigned int columns)
{
  unsigned int i, j;
  
  for (i=0; i<=rows; i++)
    for (j=0; j<=columns; j++)
      *result++=*(m1+(columns+1)*j+i);
}

/* matrix [2,2] inversion */
void minv2(int *matrix, long *result)
{
  int det;
  unsigned int i;

  det=((long)*matrix**(matrix+3)-(long)*(matrix+1)**(matrix+2))>>15;
  if (det==0) det=1;

  *result++=(((long)*(matrix+3))<<15)/det;
  *result++=(((long)*(matrix+1))<<15)/det;
  *result++=(((long)*(matrix+2))<<15)/det;
  *result++=(((long)*matrix)<<15)/det;
}


void choice_P(int *m, int *result, unsigned int columns)
{
  *result++=*m;
  *result++=*(m+1);
  *result++=*(m+columns+1);
  *result++=*(m+columns+2);
}


void choice_x(int *m, int *result)
{
  *result++=*m++;
  *result=*m;
}