text: /* Copyright 2013. The Regents of the University of California.
   * All rights reserved. Use of this source code is governed by 
   * a BSD-style license which can be found in the LICENSE file.
   *
   * Authors:
   * 2013 Martin Uecker <uecker@eecs.berkeley.edu>
   * 2014 Jonathan Tamir <jtamir@eecs.berkeley.edu>
   *
   *
   * Peter J. Shin, Peder E.Z. Larson, Michael A. Ohliger, Michael Elad,
   * John M. Pauly, Daniel B. Vigneron and Michael Lustig, Calibrationless
   * Parallel Imaging Reconstruction Based on Structured Low-Rank Matrix 
   * Completion, Magn Reson Med. Epub (2014)
   *
   * Zhongyuan Bi, Martin Uecker, Dengrong Jiang, Michael Lustig, and Kui Ying.
   * Robust Low-rank Matrix Completion for sparse motion correction in auto 
   * calibration PI. Annual Meeting ISMRM, Salt Lake City 2013, 
   * In Proc. Intl. Soc. Mag. Recon. Med 21; 2584 (2013)
   */
  
  #include <stdlib.h>
  #include <assert.h>
  #include <math.h>
  #include <complex.h>
  
  #include "num/lapack.h"
  #include "num/linalg.h"
  #include "num/multind.h"
  #include "num/flpmath.h"
  #include "num/casorati.h"
  
  #include "misc/misc.h"
  #include "misc/debug.h"
  #include "misc/mri.h"
  
  #include "sake.h"
  
  #undef DIMS // FIXME
  #define DIMS 16
  
  #if 0
  static float thresh(float lambda, float x)
  {
  	float norm = fabs(x);
  	float red = norm - lambda;
  	return (red > 0.) ? ((red / norm) * (x)) : 0.;
  }
  #endif
  
  static complex float cthresh(float lambda, complex float x)
  {
  	float norm = cabsf(x);
  	float red = norm - lambda;
  	return (red > 0.) ? ((red / norm) * x) : 0.;
  }
  
  
  
  static void robust_consistency(float lambda, const long dims[5], complex float* dst, const complex float* pattern, const complex float* kspace)
  {
  	assert(1 == dims[4]);
  
  	size_t size = md_calc_size(5, dims);
  
  	for (unsigned int i = 0; i < size; i++)
  		if (1. == pattern[i % (size / dims[3])])
  			dst[i] = kspace[i] + cthresh(lambda, dst[i] - kspace[i]);
  }
  
  #if 1
  #define RAVINE
  #endif
  #ifdef RAVINE
  static void ravine(unsigned int N, const long dims[N], float* ftp, complex float* xa, complex float* xb)
  {
          float ft = *ftp;
          float tfo = ft;
  
          ft = (1.f + sqrtf(1.f + 4.f * ft * ft)) / 2.f;
          *ftp = ft;
  
  	md_swap(N, dims, xa, xb, CFL_SIZE);
  	complex float val = (1.f - tfo) / ft - 1.f;
  
  	long dims1[N];
  	md_singleton_dims(N, dims1);
  
  	long strs1[N];
  	long strs[N];
  	md_calc_strides(N, strs1, dims1, CFL_SIZE);
  	md_calc_strides(N, strs, dims, CFL_SIZE);
  
  	md_zfmac2(N, dims, strs, xa, strs1, &val, strs, xa);
  	val *= -1.;
          md_zfmac2(N, dims, strs, xa, strs1, &val, strs, xb);
  }
  #endif
  
  
  
  
  
  static void lowrank(float alpha, int D, const long dims[D], complex float* matrix)
  {
  	assert(1 == dims[MAPS_DIM]);
  
  	debug_printf(DP_DEBUG3, "mat_dims = \t");
  	debug_print_dims(DP_DEBUG3, D, dims);
  
  	long kern_min[4] = { 6, 6, 6, dims[COIL_DIM] };
  	long kern_dims[D];
  
  	md_set_dims(D, kern_dims, 1);
  	md_min_dims(4, ~0u, kern_dims, kern_min, dims);
  
  	debug_printf(DP_DEBUG3, "kern_dims = \t");
  	debug_print_dims(DP_DEBUG3, D, kern_dims);
  
  	long calmat_dims[2];
  	casorati_dims(D, calmat_dims, kern_dims, dims);
  
  	debug_printf(DP_DEBUG3, "calmat_dims = \t");
  	debug_print_dims(DP_DEBUG3, 2, calmat_dims);
  
  	complex float* calmat = md_alloc(2, calmat_dims, CFL_SIZE);
  
  	long str[D];
  	md_calc_strides(D, str, dims, CFL_SIZE);
  
  	casorati_matrix(D, kern_dims, calmat_dims, calmat, dims, str, matrix);
  
  	int N = calmat_dims[0];
  	int M = calmat_dims[1];
  
  	debug_printf(DP_INFO, "%dx%d\n", N, M);
  
  
  	if (-1. != alpha) {
  
  		long dimsU[2] = { N, N };
  		long dimsV[2] = { M, M };
  
  		complex float* U = md_alloc(2, dimsU, CFL_SIZE);
  		complex float* VT = md_alloc(2, dimsV, CFL_SIZE);
  		//	complex float* U = create_cfl("U", 2, dimsU);
  		//	complex float* VT = create_cfl("VT", 2, dimsV);
  		float* S = xmalloc(MIN(N, M) * sizeof(float));
  
  		debug_printf(DP_INFO, "SVD..\n");
  
  		//lapack_svd(N, M, (complex float (*)[M])U, (complex float (*)[N])VT, S, (complex float (*)[N])calmat);
  		lapack_svd_econ(N, M, (complex float (*)[M])U, (complex float (*)[N])VT, S, (complex float (*)[N])calmat); // CHECK
  
  		debug_printf(DP_INFO, "done.\n");
  
  		// put it back together
  		long dimU2[2] = { N, MIN(N, M) };
  		long dimV2[2] = { MIN(N, M), M };
  		complex float* U2 = md_alloc(2, dimU2, CFL_SIZE);
  		complex float* V2 = md_alloc(2, dimV2, CFL_SIZE);
  		md_resize(2, dimU2, U2, dimsU, U, CFL_SIZE);
  		md_resize(2, dimV2, V2, dimsV, VT, CFL_SIZE);
  
  		for (int i = 0; i < M; i++) {
  
  			//		printf("%f\t", S[i]);
  
  			for (int j = 0; j < MIN(N, M); j++)
  				//V2[i * MIN(N, M) + j] *= thresh(alpha, S[j]);
  				V2[i * MIN(N, M) + j] *= (j < alpha * (float)MIN(N, M)) ? S[j] : 0.; //  thresh(alpha, S[j]);
  		}
  
  		mat_mul(M, MIN(M, N), N, (complex float (*)[N])calmat,
  				(const complex float (*)[MIN(M, N)])V2, (const complex float (*)[N])U2);
  
  		md_free(U);
  		md_free(U2);
  		md_free(VT);
  		md_free(V2);
  		free(S);
  	}
  
  	md_clear(D, dims, matrix, CFL_SIZE);
  	casorati_matrixH(D, kern_dims, dims, str, matrix, calmat_dims, calmat);
  	md_zsmul(D, dims, matrix, matrix, 1. / (double)md_calc_size(3, kern_dims)); // FIXME: not right at the border
  
  	md_free(calmat);
  }
  
  
  
  void lrmc(float alpha, int iter, float lambda, int N, const long dims[N], complex float* out, const complex float* in)
  {
  	long dims1[N];
  	md_select_dims(N, ~COIL_FLAG, dims1, dims);
  
  	md_copy(N, dims, out, in, CFL_SIZE);
  
  	complex float* pattern = md_alloc(N, dims1, CFL_SIZE);
  
  	//assert(5 == N);
  	estimate_pattern(N, dims, COIL_FLAG, pattern, in);
  
  	complex float* comp = md_alloc(N, dims1, CFL_SIZE);
  	md_zfill(N, dims1, comp, 1.);
  
  	lowrank(-1., N, dims1, comp);
  
  #ifdef RAVINE
  	complex float* o = md_alloc(N, dims, CFL_SIZE);
  	md_clear(N, dims, o, CFL_SIZE);
  	float fl = 1.;
  #endif
  
  	long strs1[N];
  	md_calc_strides(N, strs1, dims1, CFL_SIZE);
  
  	long strs[N];
  	md_calc_strides(N, strs, dims, CFL_SIZE);
  	
  
  	for (int i = 0; i < iter; i++) {
  
  		debug_printf(DP_INFO, "%d\n", i);
  
  		if (-1. != lambda)
  			robust_consistency(lambda, dims, out, pattern, in);
  		else
  			data_consistency(dims, out, pattern, in, out);
  
  		lowrank(alpha, N, dims, out);
  		md_zdiv2(N, dims, strs, out, strs, out, strs1, comp);
  #ifdef RAVINE
  		ravine(N, dims, &fl, out, o);
  #endif	
  	}
  	
  	debug_printf(DP_INFO, "Done.\n");
  #ifdef RAVINE
  	md_free(o);
  #endif
  	md_free(comp);
  	md_free(pattern);
  }
  
  
  