/* ************************** 
   cnv2.c

   Created on: June 11, 2011
   Author: Michael Hirsch
   ************************** */
#ifndef RESAMPLING_C
#define RESAMPLING_C

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <complex.h>
#include <fftw3.h>
#include <sys/time.h>
#include <string.h>

#include "csparse.h"
#include "resampling.h"
#include "utils.h"


matrix * csparse2matrix(cs * in){

  int p, j, m, n, nzmax, nz, *Ap, *Ai ;
  double *Ax ;

  if (!A) { printf ("(null)\n") ; return (0) ; }
  m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
  nzmax = A->nzmax ; nz = A->nz ;

  matrix * out = (matrix*) malloc(sizeof(out));;
  out->size.m = m;
  out->size.n = n;
  out->mat = (real*) calloc(m * n, sizeof(out->mat));

  if (nz < 0){
    printf ("%d-by-%d, nzmax: %d nnz: %d, 1-norm: %g\n", m, n, nzmax,
	    Ap [n], cs_norm (A)) ;

    for (j = 0 ; j < n ; j++){
      for (p = Ap [j] ; p < Ap [j+1] ; p++){
	out->mat[ IDX(p,j,out->size.n) ] = Ax[p];
      }
    }
  }
  else {
    for (p = 0 ; p < nz ; p++){
      out->mat[ IDX(Ai [p], Ap [p], out->size.n) ] += Ax [p];
    }
  }
  return out;
}

cs * matrix2csparse(matrix * in){

  cs * out;
  if (!in->mat) return (NULL) ;

  out = cs_spalloc (in->size.m, in->size.n, 1, 1, 1) ;

  for (int i=0; i < in->size.m; i++){
    for (int j=0; j < in->size.n; j++){
      if (in->mat[ IDX(i,j,in->size.n) ] != 0){
	  if (!cs_entry (out, i, j, in->mat[ IDX(i,j,in->size.n) ])){
	    return (cs_spfree (out)) ;
	  }
	}
    }
  }
  return out ;
}

resample2mat * resample2_matrix(size sin, size sout){

  // Allocate memory and save resample matrices in struct
  resample2mat * resampleobj = (resample2mat*) malloc(sizeof(resample2mat));

  resampleobj->R1 = resample_matrix();
  resampleobj->R2 = resample_matrix();

  resampleobj->R1tp = cs_transpose(resampleobj->R1, 1);
  resampleobj->R2tp = cs_transpose(resampleobj->R2, 1);

  resampleobj->sin = sin;
  resampleobj->sout = sout;

  return resampleobj;
}

cs * resample_matrix (int in, int out){

  // Generate resample matrix
  // D = kron(speye(m), ones(n, 1)') * kron(speye(n), ones(m, 1))/m;  

  
};

matrix * resample2(resample2mat * R, matrix * in){
  // R(:,:,i) = (a.D2 * (a.D1 * B(:,:,i))')';

  cs * tmp0 = matrix2csparse(in);

  cs * tmp1 = cs_multiply(R->R1, tmp0);
  cs * tmp2 = cs_transpose(tmp1, 1);
  cs * tmp3 = cs_multiply(R->R2, tmp2);
  cs * tmp4 = cs_transpose(tmp3, 1);

  matrix * out = csparse2matrix(tmp4)

  cs_spfree(tmp0);
  cs_spfree(tmp1);
  cs_spfree(tmp2);
  cs_spfree(tmp3);
  cs_spfree(tmp4);

  return out;
}


matrix * resample2tp(resample2mat * R, matrix * in){
  // R(:,:,i) = (a.D2' * (a.D1' * B(:,:,i))')';

  cs * tmp0 = matrix2csparse(in);

  cs * tmp1 = cs_multiply(R->R1tp, in);
  cs * tmp2 = cs_transpose(tmp1, 1);
  cs * tmp3 = cs_multiply(R->R2tp, tmp2);
  cs * tmp4 = cs_transpose(tmp3, 1);

  matrix * out = csparse2matrix(tmp4)

  cs_spfree(tmp0);
  cs_spfree(tmp1);
  cs_spfree(tmp2);
  cs_spfree(tmp3);
  cs_spfree(tmp4);

  return out;
}

#endif