#include "matrix.hpp"

void
Matrix::init(size_t nrow,size_t ncol){
  if(data!=nullptr) delete[] data;
  nr=nrow;
  nc=ncol;
  nc_avx=(nc-1)/4+1;
  nc_full=4*nc_avx;
  data=(double*)new __m256d[nr*nc_full];
}

//---------------
// Matrix::clear
//---------------

void
Matrix::clear(){
  __m256d* avx=(__m256d*)data;
  for(size_t i=0;i<nr*nc_avx;++i){
    avx[i]=zeros;
  }
}

void
Matrix::display() const{
  for(size_t i=0;i<nr;++i){
    for(size_t j=0;j<nc;++j){
      cout<<get(i,j)<<'\t';
    }
    cout<<endl;
  }
}

void
Matrix::swap_lines(size_t i,size_t j){
  __m256d* avx_i=get_avx_row(i);
  __m256d* avx_j=get_avx_row(j);
  for(size_t k=0;k<nc_avx;++k){
    __m256d a=*avx_i;
    *avx_i=*avx_j;
    *avx_j=a;
    ++avx_i;
    ++avx_j;
  }
}

void
Matrix::mul_line(size_t i,double a){
  __m256d b=_mm256_set1_pd(a);
  __m256d* avx=get_avx_row(i);
  for(size_t k=0;k<nc_avx;++k){
    *avx=_mm256_mul_pd(*avx,b);
    ++avx;
  }
}

void
Matrix::add_mul_line(size_t i,size_t j,double a){
  __m256d b=_mm256_set1_pd(a);
  __m256d* avx_i=get_avx_row(i);
  __m256d* avx_j=get_avx_row(j);
  for(size_t k=0;k<nc_avx;++k){
    *avx_i=_mm256_fmadd_pd(*avx_j,b,*avx_i);
    ++avx_i;
    ++avx_j;
  }
}


double
Matrix::Gauss(){
  double det=1;
  size_t np=0; //np=0
  for(size_t j=0;j<nc;++j){
    for(size_t p=np;p<nr;++p){
      double c=get(p,j);
      if(c!=0){
        det*=c;
        mul_line(p,1.0/c);
        for(size_t k=0;k<nr;++k){
          if(k!=p){
            add_mul_line(k,p,-get(k,j));
          }
        }
        if(p!=np){
          swap_lines(np,p);
          det*=-1;
        }
        ++np;
        break;
      }
    }
  }
  return det;
}

double
Matrix::get_diag_square_sym(size_t i) const{
  AvxBlock b;
  b.avx=zeros;
  const __m256d* avx=get_avx_row(i);
  for(size_t k=0;k<nc_avx;++k){
    b.avx=_mm256_fmadd_pd(*avx,*avx,b.avx);
    ++avx;
  }
  return b.data[0]+b.data[1]+b.data[2]+b.data[3];
}