#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "matrice.h"
#include "lbl_array.h"
#include "lsq_fix.h"

/*---------------------------------------------------------*/

int mat_show(char *msg,matrice Mat)
{
  int i,j,r,c;

  r = Mat.nlig;
  c = Mat.ncol;
  printf("\n%s : %d ligs, %d cols\n\n",msg,r,c);
  for (i=0; i<r; i++) {
      for (j=0; j<c; j++)
          printf("%.6lf ",e(Mat,i,j));
      printf("\n");
  }
  return(1);
}

/*---------------------------------------------------------*/

int mat_prod(double *A, double *B, double *C, int n, int p, int m)
{
 double *ptA, *ptB, *ptC, *finC, *finA, *finB, *dA, *dB ;
 double *CC ;
 int nm ;
 int a_echanger ;

 nm = n*m ;

 CC = C ;
 a_echanger = 0 ;

 dA = A ;
 dB = B ;
 finB = B + m ;

 for (ptC=CC, finC=ptC+nm ; ptC<finC ; ptC++) {
     (*ptC) = 0.0 ;
     for (ptA=dA, ptB=dB, finA=ptA+p ; ptA<finA ; ptA++, ptB+=m) {
         (*ptC) += (*ptA) * (*ptB) ;
     }
     dB++ ;
     if (dB==finB) {
        dB  = B ;
        dA += p ;
     }
 }

 if (a_echanger) {
    for (ptA=C, ptB=CC, finA=ptA+nm ; ptA<finA ; ptA++, ptB++) (*ptA) = (*ptB) ;
    free(CC) ;
 }
 return(1) ;
}

/*---------------------------------------------------------*/

int mat_unit(double *A, int r)
{
 double *ptA;

 for (ptA=A; ptA<A+r*r ; ptA++)
     (*ptA) = 0.0;
 for (ptA=A; ptA<A+r*r ; ptA+=(r+1))
     (*ptA) = 1.0;

 return(1);
}

/*---------------------------------------------------------*/

int mat_plus(double *A, double *B, double *C, int r, int c)
{
 double *ptA, *ptB, *ptC;

 for (ptA=A , ptB=B , ptC=C; ptA<A+r*c , ptB<B+r*c , ptC<C+r*c; ptA++ , ptB++ , ptC++)
     (*ptC) = (*ptA) + (*ptB);

 return(1);
}

/*---------------------------------------------------------*/

int mat_moins(double *A, double *B, double *C, int r, int c)
{
 double *ptA, *ptB, *ptC;

 for (ptA=A , ptB=B , ptC=C; ptA<A+r*c , ptB<B+r*c , ptC<C+r*c; ptA++ , ptB++ , ptC++)
     (*ptC) = (*ptA) - (*ptB);

 return(1);
}

/*---------------------------------------------------------*/

int mat_scal_prod(double *A, int r, int c, double scal)
{
 double *ptA;

 for (ptA=A; ptA<A+r*c; ptA++)
     (*ptA) = ((*ptA)*scal);

 return(1);
}

/*---------------------------------------------------------*/

int mat_transpose(double *A, double *B, int r, int c)
{
 double *ptA, *ptB, *finB, *dB ;

 dB = B;
 finB = B+r*c;
 ptB = dB;
 for (ptA=A; ptA<A+r*c; ptA++) {
     (*ptB) = (*ptA);
     ptB += r;
     if (ptB >= finB) {
        dB++;
        ptB = dB;
        finB = dB+r*c;
     }
 }
 return(1);
}

/*---------------------------------------------------------*/

int mat_copy(double *A, double *B, int r, int c)
{
 int i;
 double *ptA, *ptB;

 ptA = A;
 ptB = B;
 for (i=0; i<r*c; i++) {
     (*ptB) = (*ptA);
     ptB++;
     ptA++;
 }

 return(1);
}

/*---------------------------------------------------------*/

void solve_syst(double *a,double *b)
{
        double det,x,y;
        double alpha,beta,gamma;

        det = a[0]*a[3]-a[1]*a[2];

        if (fabs(det) > 1e-32) {
                x = (a[3]*b[0]-a[2]*b[1])/det;
                y = (a[0]*b[1]-a[1]*b[0])/det;
        }
        else {
                alpha = a[0]+a[2];
                beta = a[1]+a[3];
                gamma = b[0]+b[1];
                det = alpha*alpha+beta*beta;
                x = alpha*gamma/det;
                y = beta*gamma/det;
        }

        /*--- return solution in b */
        b[0] = x;
        b[1] = y;
}

/*---------------------------------------------------------*/

double Norme(double *A, int N)
{
 double norme=0.0, absolu ;
 double *pt, *fin ;

 for(pt=A, fin=pt+N ; pt<fin ; pt++)
 {
  absolu = Fabs((*pt)) ;
  norme = Max(absolu,norme) ;
 }
 return(norme) ;
}

/*-----------------------------------------------------------*/

int mat_prod_S(matrice A,matrice B,matrice C)
{
  if ((A.ncol != B.nlig) || (A.nlig != C.nlig) || (B.ncol != C.ncol) ) {
     printf("Erreur de dimensions dans mat_prod\n");
     return(0);
  }
  return(mat_prod(A.valeur,B.valeur,C.valeur,A.nlig,A.ncol,B.ncol));
}

/*-----------------------------------------------------------*/

int mat_unit_S(matrice A)
{
  if (A.nlig != A.ncol) {
     printf("matrice non carree dans mat_unit\n");
     return(0);
  }
  return(mat_unit(A.valeur,A.nlig));
}

/*-----------------------------------------------------------*/

int mat_plus_S(matrice A,matrice B,matrice C)
{
  int r,c;

  r = A.nlig;
  c = A.ncol;
  if ((B.nlig != r) || (C.nlig != r) || (B.ncol != c) || (C.ncol != c)) {
     printf("Matrices de dimensions differentes dans mat_plus\n");
     return(0);
  }

  return(mat_plus(A.valeur,B.valeur,C.valeur,A.nlig,A.ncol));
}

/*-----------------------------------------------------------*/

int mat_moins_S(matrice A,matrice B,matrice C)
{
  int r,c;

  r = A.nlig;
  c = A.ncol;
  if ((B.nlig != r) || (C.nlig != r) || (B.ncol != c) || (C.ncol != c)) {
     printf("Matrices de dimensions differentes dans mat_moins\n");
     return(0);
  }
  return(mat_moins(A.valeur,B.valeur,C.valeur,A.nlig,A.ncol));
}

/*-----------------------------------------------------------*/

int mat_scal_prod_S(matrice A,double scal)
{
  return(mat_scal_prod(A.valeur,A.nlig,A.ncol,scal));
}

/*-----------------------------------------------------------*/

int mat_transpose_S(matrice Mat,matrice Mat_transp)
{
  if ((Mat.nlig != Mat_transp.ncol) || (Mat.ncol != Mat_transp.nlig)) {
     printf("Erreur de dimensions dans mat_transpose\n");
     return(0);
  }
  return(mat_transpose(Mat.valeur,Mat_transp.valeur,Mat.nlig,Mat.ncol));
}

/*-----------------------------------------------------------*/

int mat_copy_S(matrice Mat,matrice Mat_copy)
{
  if ((Mat.nlig != Mat_copy.nlig) || (Mat.ncol != Mat_copy.ncol)) {
     printf("Matrices de dimensions differentes dans mat_copy\n");
     return(0);
  }
  return(mat_copy(Mat.valeur,Mat_copy.valeur,Mat.nlig,Mat.ncol));
}

/*-----------------------------------------------------------*/

int mat_new_S(matrice *Mat,int nlig,int ncol)
{
  int i,j;

  if ( (Mat->valeur = (double *)malloc(nlig*ncol*sizeof(double))) == NULL) {
     printf("erreur d'allocation memoire dans mat_new\n");
     return(0);
  }

  Mat->nlig = nlig;
  Mat->ncol = ncol;
  for (i=0; i<nlig; i++)
      for (j=0; j<ncol; j++)
          e((*Mat),i,j) = 0.0;

  return(1);
}

/*-----------------------------------------------------------*/

double inv22(double *a)
{
        double det,a11,a12,a21,a22;

        det = a[0]*a[3]-a[1]*a[2];
        if (fabs(det) > 1e-32) {
                a11 = a[3]/det;
                a12 =-a[2]/det;
                a21 =-a[1]/det;
                a22 = a[0]/det;
                a[0] = a11;
                a[1] = a12;
                a[2] = a21;
                a[3] = a22;
        }
        return(det);
}

/*-----------------------------------------------------------*/

void init_matrices()
{
        mat_new_S(&X,NB_STATE,1);
        mat_new_S(&P,NB_STATE,NB_STATE);
        mat_new_S(&F,NB_STATE,NB_STATE); mat_unit_S(F);
        mat_new_S(&J,NB_STATE,NB_INPUT);
        mat_new_S(&JJ,NB_STATE,NB_INPUT);
        mat_new_S(&H,1,NB_STATE);
        mat_new_S(&K,NB_STATE,1);
        mat_new_S(&R,NB_OBS,NB_OBS); e(R,0,0) = TOF_VAR;
        mat_new_S(&RZ,NB_OBS,NB_OBS); e(RZ,0,0) = DEPTH_VAR;
        mat_new_S(&Sinv,NB_OBS,NB_OBS);
        mat_new_S(&INNOV,NB_OBS,NB_OBS);
        mat_new_S(&Cin,NB_INPUT,NB_INPUT);
        mat_new_S(&D,NB_OBS,NB_INPUT);
        mat_new_S(&est,5,NB_DATA_EST);
        e(Cin,0,0) = YAW_VAR;
        e(Cin,1,1) = PITCH_VAR;
        e(Cin,2,2) = SPEED_VAR;
        mat_new_S(&Q,NB_STATE,NB_STATE);
        e(Q,0,0) = e(Q,1,1) = Qxy;
        e(Q,2,2) = Qz;
        e(Q,3,3) = e(Q,4,4) = Qvc;
        e(Q,5,5) = Qdu;
        e(Q,6,6) = Qdpsi;

        mat_new_S(&covar,2,2);

        mat_new_S(&JCinJt,NB_STATE,NB_STATE);
        mat_new_S(&FPFt,NB_STATE,NB_STATE);
        mat_new_S(&Ft,NB_STATE,NB_STATE);
        mat_new_S(&FP,NB_STATE,NB_STATE);
        mat_new_S(&Jt,NB_INPUT,NB_STATE);
        mat_new_S(&JCin,NB_STATE,NB_INPUT);
        mat_new_S(&FPFtpJCinJt,NB_STATE,NB_STATE);

        mat_new_S(&KH,NB_STATE,NB_STATE);
        mat_new_S(&ImKH,NB_STATE,NB_STATE);
        mat_new_S(&ImKHP,NB_STATE,NB_STATE);
        mat_new_S(&I,NB_STATE,NB_STATE); mat_unit_S(I);

        mat_new_S(&HPHt,NB_OBS,NB_OBS);
        mat_new_S(&Ht,NB_STATE,NB_OBS);
        mat_new_S(&HP,NB_OBS,NB_STATE);

        mat_new_S(&PHt,NB_STATE,NB_OBS);

        mat_new_S(&KK,NB_STATE,NB_OBS);
        mat_new_S(&XpKinnov,NB_STATE,1);

        mat_new_S(&Dt,NB_INPUT,NB_OBS);
        mat_new_S(&DCinDt,NB_OBS,NB_OBS);
        mat_new_S(&S,NB_STATE,NB_OBS);
        mat_new_S(&HS,NB_OBS,NB_OBS);
        mat_new_S(&HSt,NB_OBS,NB_OBS);
        mat_new_S(&DCin,NB_OBS,NB_INPUT);

        mat_new_S(&PHtpSSinv,NB_STATE,NB_OBS);

        mat_new_S(&St,NB_OBS,NB_STATE);
        mat_new_S(&HPpSt,NB_OBS,NB_STATE);
        mat_new_S(&KHPpSt,NB_STATE,NB_STATE);
        mat_new_S(&PmKHPpSt,NB_STATE,NB_STATE);

        mat_new_S(&Y,MAX_BEACON_NUMBER,1);
        mat_new_S(&W,MAX_BEACON_NUMBER,2);
        mat_new_S(&Wt,2,MAX_BEACON_NUMBER);
        mat_new_S(&temp,2,MAX_BEACON_NUMBER);
        mat_new_S(&dy,MAX_BEACON_NUMBER,1);
        mat_new_S(&res,1,1);
        mat_new_S(&Ninv,MAX_BEACON_NUMBER,MAX_BEACON_NUMBER);
        mat_new_S(&WtNinv,2,MAX_BEACON_NUMBER);
        mat_new_S(&dyt,1,MAX_BEACON_NUMBER);
        mat_new_S(&dytNinv,1,MAX_BEACON_NUMBER);

        mat_new_S(&a,2,1);
        mat_new_S(&x,MAX_BEACON_NUMBER,4);
        mat_new_S(&y,MAX_BEACON_NUMBER,1);
        mat_new_S(&noise,MAX_BEACON_NUMBER,MAX_BEACON_NUMBER);

        mat_new_S(&Covariance,2,2);
        mat_new_S(&Hessien,2,2);
        mat_new_S(&Derivee,2,1);
        mat_new_S(&essai,2,1);
        mat_new_S(&da,2,1);
        mat_new_S(&sol,2,1);
}