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

   This file is a part of the

             COMPLEX PSEUDO-RAW SAR IMAGE PROCESSING TOOLBOX

   Copyright (C) 2017 Rémy Abergel (Remy.Abergel AT parisdescartes.fr), Loïc
   Denis (Loic.Denis AT univ-st-etienne.fr), Saïd Ladjal (Said.Ladjal AT
   telecom-paristech.fr) and Florence Tupin (Florence.Tupin AT
   telecom-paristech.fr).

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.

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

/*                    ************************************
                      * MAIN DOCUMENTATION OF THE MODULE *
                      ************************************

   Compilation (using gcc):
   ========================

   To simplify interfacing (for instance with Matlab) the source file
   'irregularsampling_kernel.c' only contains the core functions of the
   resampling algorithms.

   A 'main' function (including argument parser and TIFF input/output support)
   corresponding to this module can be found into the source file
   'irregularsampling.c', it can be compiled using the following command line.

   gcc -O3 irregularsampling.c irregularsampling_kernel.c tools.c tiffread.c tiffwrite.c -lfftw3 -lm -ltiff -o irregularsampling


   Short description of this module:
   =================================

   The module 'irregularsampling' is an implementation of the irregular
   resampling scheme proposed in the research paper [1] (Algorithm 1), and also
   described in [2] (see Algorithm 1).

   [1] R. Abergel, S. Ladjal, F. Tupin and J.-M. Nicolas, ``A Complex Spectrum
       Based SAR Image Resampling Method with Restricted Target Sidelobes and
       Statistics Preservation'', in proceedings of the 2017 IEEE International
       Geoscience and Remote Sensing Symposium (IGARSS 2017).

   [2] R. Abergel, L. Denis, S. Ladjal and F. Tupin, ``Sub-pixellic Methods for
       Sidelobes Suppression and Strong Targets Extraction in Single Look
       Complex SAR Images'', preprint HAL-IMT, submitted in July 2017.

   Note: the work [2] corresponds to the revision of the manuscript entitled
   ``Sidelobes Suppression and Strong Targets Extraction in Single Look Complex
   SAR Images with Several Applications'' that we submitted in July 2017 (we
   changed the title according to one Reviewer's recommandation).

   This irregular resampling algorithm can be used to process pseudo-raw SAR
   images acquired in stripmap mode (pseudo-raw means non-apodized and
   non-oversampled SAR images, see [1,2] for more details). It consists in
   resampling the input (complex-valued) pseudo-raw image over a non-regular
   grid such as the resampled image does not exhibits sidelobes in the vicinity
   of strong target responses.

   It was observed in [1,2] that this scheme preserves the spatial resolution of
   the input pseudo-raw image, as well as the signal statistics in fully
   developped speckle area.


   Mathematical definitions:
   =========================

   Given an input pseudo-raw image u0 with horizontal (width) and vertical
   (height) dimensions noted 'nx' and 'ny', the irregularly resampled images
   computed by this algorithm is the image v0 with same dimensions as u0,
   defined by

   for all (k,l) in {0,...,nx-1} x {0,...,ny-1},

   v0(k,l) = U0(k-Tx(k,l),l-Ty(k,l)).

   where

     + U0 denotes the Shannon interpolate of u0 (U0 is a function from R^2 to C)
     + Tx(k,l) and Ty(k,l) are two real numbers between -0.5 and 0.5.

   Therefore, v0 corresponds to the sampling of the Shannon interpolate U0 over
   the non-regular grid defined by

   { (k-Tx(k,l) , l-Ty(k,l)) , for (k,l) in {0,...,nx-1} x {0,...,ny-1} }.

   We refer to the research paper [1,2] (see references above) for more details
   about this algorithm, as well as numerical examples of images that it
   provides.


   Inputs/outputs:
   ===============

   Complex-valued images (u0 and v0) are stored as a stack of 2 TIFF-float
   images, the first (respectively the second) image of the stack corresponds to
   the real (respectively imaginary) part of the complex signal.

   Float precision for the pixel values is non-mandatory for input u0 (8-bit, 16
   bits formats are supported), however, using float precision is strongly
   recommanded since saturation strongly affects the Shannon interpolate of the
   image.

   The optional output T is also a stack of 2 TIFF-floats images. The first
   (respectively the second) image of the stack correspond to the horizontal
   component Tx (respectively the vertical component Ty) of the dense
   displacement field T = (Tx,Ty) used to defined the irregular resampling grid.

   'K' and 'Ntransl' are integer inputs of the algorithm. 'K' is a locality
   parameter and 'Ntransl' denotes the number of tested displacements, such as
   the set of tested displacements (= candidate values of Tx(k,l) and Ty(k,l)
   for all (k,l)) is

             {-0.5 + id/Ntransl, for id in {0,...,Ntransl-1}}.

   See [1,2] for more details about those two parameters.

 */

#include <math.h>
#include <fftw3.h>

#ifndef MATLAB_MEX_FILE // STANDARD COMPILATION

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define MATLAB_DISPLAY 0

#else // MEX COMPILATION (use several tricks for Matlab printf)

#include <mex.h>
#include <fftw3.h>
#define fflush(stdout) mexEvalString("pause(0);") // force Matlab to display message in real time (otherwise printf messages are displayed at the end of the mex execution)
#define MATLAB_DISPLAY 1

#endif

#define ASSERT_ALLOC(cmd) if(NULL == (cmd)) { printf("Not enough memory\n"); exit(EXIT_FAILURE); }

/**************/
/* prototypes */
/**************/

// internal modules
static void find_displacement(int*,int*,int,int,int,int,int,int,fftw_complex**,fftw_complex**);
static void compute_v0(fftw_complex*,int,int,int,int,fftw_complex*,fftw_complex*,int,int,int,char);
void irregularsampling(fftw_complex*,fftw_complex*,fftw_complex*,int,int,int,int,char);

// external modules (see source file 'tools.c')
extern void horizontal_translation(fftw_complex*,fftw_complex*,int,int,double,fftw_plan);
extern void vertical_translation(fftw_complex*,fftw_complex*,int,int,double,fftw_plan);
extern double sinc(double);


/********************/
/* kernel functions */
/********************/

/* find_displacement: compute the value of Tx(x0,y0) & Ty(x0,y0), the horizontal
   & vertical displacements at location (x0,y0).

   Inputs description:
   ===================

   + 'K 'and 'Ntransl': the locality parameter and the number of tested
   translations (see above the 'Input/Output' section of the main documentation
   of this module).

   + 'nx' and 'ny' denotes the horizontal (width) and vertical (heigh) dimensions of
   the image

   + 'u0_tx' (respectively 'u0_ty') is a stack of images, such as, for any
     integer index integer index 'id' in {0,..,Ntransl-1}, u0_tx[id] (respectively
     u0_ty[id]) contains the horizontal (respectively vertical) translation of the
     pseudo-raw image u0 with subpixellic translation step

     t = -0.5+(double)id/(double)Ntransl.

     More precisely, for all integer pixel positions (x,y) in
     {0,...,nx-1}x{0,...,ny-1}, we have

         u0_tx[id][y*nx+x] = U0(x-t,y), and u0_tx[id][y*nx+x] = U0(x,y-t),

     where U0 denotes the Shannon interpolate of the image u0.

   Outputs description:
   ====================

   It sets the values of 'id_tx' and 'id_ty', such as the computed displacements
   are

   Tx(x0,y0) = -0.5 + (double)id_tx/(double)Ntransl,
   Ty(x0,y0) = -0.5 + (double)id_ty/(double)Ntransl.

*/
static void find_displacement(id_tx,id_ty,x0,y0,K,Ntransl,nx,ny,u0_tx,u0_ty)
     int *id_tx,*id_ty,x0,y0,K,Ntransl,nx,ny;
     fftw_complex **u0_tx,**u0_ty;
{
  int x,y,id_t,dx,dy,dmax_re,dmax_im;
  double tv_re,tv_im,tv,tv_min,ampl,ampl_max_re,ampl_max_im;

  // find horizontal component of the displacement //
  y = y0;
  tv_min = INFINITY;
  dmax_re = dmax_im = 0; // to avoid gcc warnings
  for(id_t=0;id_t<Ntransl;id_t++) {
    // find the location of the max of the amplitude of the real and imaginary
    // parts of the signal u0_tx[id_t] in the vicinity of (x0,y0)
    ampl_max_re = ampl_max_im = 0.;
    for(dx=-K;dx<=K;dx++) {
      x = (nx+x0+dx)%nx;
      ampl = fabs(u0_tx[id_t][y*nx+x][0]);
      if(ampl_max_re < ampl) {
	ampl_max_re = ampl;
	dmax_re = dx;
      }
      ampl = fabs(u0_tx[id_t][y*nx+x][1]);
      if(ampl_max_im < ampl) {
	ampl_max_im = ampl;
	dmax_im = dx;
      }
    }
    // compute TV-score of the real & imaginary parts of u0_tx[id_t] in the
    // vicinity of (x0,y0)
    tv_re = tv_im = 0.;
    for(dx=-K;dx<=K;dx++) {
      if((dx != dmax_re) && (dx+1 != dmax_re)) tv_re += fabs(u0_tx[id_t][y*nx+(nx+x0+dx+1)%nx][0] - u0_tx[id_t][y*nx+(nx+x0+dx)%nx][0]);
      if((dx != dmax_im) && (dx+1 != dmax_im)) tv_im += fabs(u0_tx[id_t][y*nx+(nx+x0+dx+1)%nx][1] - u0_tx[id_t][y*nx+(nx+x0+dx)%nx][1]);
    }
    // check if this translation improves tv_min //
    tv = tv_re + tv_im;
    if(tv_min > tv) {
      tv_min = tv;
      (*id_tx) = id_t;
    }
  }

  // find vertical component of the displacement //
  x = x0;
  tv_min = INFINITY;
  for(id_t=0;id_t<Ntransl;id_t++) {
    // find the location of the max of the amplitude of the real and imaginary
    // parts of the signal u0_ty[id_t] in the vicinity of (x0,y0)
    ampl_max_re = ampl_max_im = 0.;
    for(dy=-K;dy<=K;dy++) {
      y = (ny+y0+dy)%ny;
      ampl = fabs(u0_ty[id_t][y*nx+x][0]);
      if(ampl_max_re < ampl) {
	ampl_max_re = ampl;
	dmax_re = dy;
      }
      ampl = fabs(u0_ty[id_t][y*nx+x][1]);
      if(ampl_max_im < ampl) {
	ampl_max_im = ampl;
	dmax_im = dy;
      }
    }
    // compute TV-score of the real & imaginary parts of u0_ty[id_t] in the
    // vicinity of (x0,y0)
    tv_re = tv_im = 0;
    for(dy=-K;dy<=K;dy++) {
      if((dy != dmax_re) && (dy+1 != dmax_re)) tv_re += fabs(u0_ty[id_t][((ny+y0+dy+1)%ny)*nx+x][0] - u0_ty[id_t][((ny+y0+dy)%ny)*nx+x][0]);
      if((dy != dmax_im) && (dy+1 != dmax_im)) tv_im += fabs(u0_ty[id_t][((ny+y0+dy+1)%ny)*nx+x][1] - u0_ty[id_t][((ny+y0+dy)%ny)*nx+x][1]);
    }
    // check if this translation improves tv_min //
    tv = tv_re + tv_im;
    if(tv_min > tv) {
      tv_min = tv;
      (*id_ty) = id_t;
    }
  }

}

/* compute_v0: compute the value of the irregularly resampled image v0 at the
   given location (x0,y0).

   Inputs description:
   ===================

   + 'Ntransl' denotes the number of tested translations (see above the
   'Input/Output' section of the main documentation of this module).

   + 'nx' and 'ny' denotes the horizontal (width) and vertical (heigh)
   dimensions of the image.

   + 'x0', 'y0' denote horizontal and vertical index position where v0 will be
   computed ((x0,y0) lies in {0,...,nx-1} x {0,...,ny-1}).

   + 'id_tx' and 'id_ty' are the horizontal and vertical displacement indexes
   computed using 'find_displacement' routine. More precisely, those indexes are
   such as

   Tx(x0,y0) = -0.5 + (double)id_tx/(double)Ntransl
   Ty(x0,y0) = -0.5 + (double)id_ty/(double)Ntransl

   + 'u0_tx' and 'u0_ty' denote the horizontal and vertical translations of u0
     using the subpixellic translation steps

     tx = -0.5 + (double)id_tx/(double)Ntransl,
     ty = -0.5 + (double)id_ty/(double)Ntransl.

     More precisely, noting U0 the Shannon interpolate of u0, we have

     for all (x,y) in {0,...,nx-1} x {0,...,ny-1},

            u0_tx[y*nx+x] = U0(x-tx,y) and u0_ty[y*nx+x] = U0(x,y-ty)

   + 'dir' should be set to 0 if nx >= ny, and any non-zero value otherwise (see
   explanation in the Output section below).

   Outputs description:
   ====================

   This function sets v0[y0*nx+x0] = U0(x0-tx,y0-ty), which means that v0 at
   position (x0,y0) is set equal to the value at position (x0-tx,y0-ty) of the
   Shannon interpolate of u0.

   Thanks to the separability of the Shannon interpolation and the precomputed
   horizontal & vertical translations (given as inputs 'u0_tx' and 'u0_ty'),
   this quantity can be computed as mono-dimensional inner product, using one of
   the two following equalities:

   v0[y0*nx+x0] = sum_{0 <= x <= nx-1} u0_ty[y0*nx+x] * sinc(x-x0+tx)       (i)
   v0[y0*nx+x0] = sum_{0 <= y <= ny-1} u0_ty[y*nx+x0] * sinc(y-y0+ty)       (ii)

   where sinc denotes the cardinal sine function defined by
   sinc(x)=sin(pi*x)/(pi*x) if 'x' is non-zero, and sinc(0)=1.

   Formula (i) amounts to evaluate at position (x0,y0) the horizontal
   translation of 'u0_ty' with subpixellic translation step tx, while formula
   (ii) amounts to evaluate at position (x0,y0) the vertical translation of
   'u0_tx' with subpixellic translation step ty.

   To reduce computation time, equality (i) should be preferred to (ii) when
   nx<ny (i.e. when input 'dir' is nonzero), and (ii) should be preferred to
   (ii) when nx >= ny (i.e. when input 'dir' is equal to).

 */
static void compute_v0(v0,x0,y0,id_tx,id_ty,u0_tx,u0_ty,nx,ny,Ntransl,dir)
     fftw_complex *v0,*u0_tx,*u0_ty;
     int x0,y0,id_tx,id_ty,nx,ny,Ntransl;
     char dir;
{
  double tx,ty,val_re,val_im,val;
  int x,y;

  // select smallest dimension for the loop //
  if(dir) { // horizontal dimension is the smallest
    tx = -0.5 + (double)id_tx/(double)Ntransl;
    val_re = val_im = 0.;
    for(x=0;x<nx;x++) {
      val = sinc(x-x0+tx);
      val_re += u0_ty[y0*nx+x][0] * val;
      val_im += u0_ty[y0*nx+x][1] * val;
    }
  }
  else { // vertical dimension is the smallest
    ty = -0.5 + (double)id_ty/(double)Ntransl;
    val_re = val_im = 0.;
    for(y=0;y<ny;y++) {
      val = sinc(y-y0+ty);
      val_re += u0_tx[y*nx+x0][0] * val;
      val_im += u0_tx[y*nx+x0][1] * val;
    }
  }

  // fill v0 at position (x0,y0) //
  v0[y0*nx+x0][0] = val_re; // real part
  v0[y0*nx+x0][1] = val_im; // imaginary part

}


/* irregularsampling: irregularly resample u0 into v0.

   Inputs description:
   ===================

   + 'u0' denotes the input pseudo-raw image.

   + 'nx' and 'ny' denotes the horizontal (width) and vertical (heigh)
      dimensions of the image.

   + 'K' and 'Ntransl' denotes the locality parameter and the number of tested
     translations (see above the 'Input/Output' section of the main
     documentation of this module).

   + 'verbose': set verbose != 0 to display algorithm progression in the user
     terminal.

   Outputs description:
   ====================

   + 'v0': the irregularly resampled image

   + 'T' : when T is allocated (NULL != T), its real part (respectively
     imaginary part) is set equal Tx (respectively Ty), that is, the horizontal
     (respectively vertical) component of the displacement field.

 */
void irregularsampling(v0,T,u0,nx,ny,Ntransl,K,verbose)
     fftw_complex *v0,*T,*u0;
     int nx,ny,Ntransl,K;
     char verbose;
{
  int x,y,adr,id_t,id_tx,id_ty,Omega=nx*ny;
  fftw_complex *dft_u0,*dft_vt,**u0_tx,**u0_ty;
  fftw_plan plan_u0,*iplan_u0_tx,*iplan_u0_ty;
  double delta;
  char dir;

  // memory allocation //
  ASSERT_ALLOC(dft_u0 = (fftw_complex*) fftw_malloc (Omega*sizeof(fftw_complex)));
  ASSERT_ALLOC(dft_vt = (fftw_complex*) fftw_malloc (Omega*sizeof(fftw_complex)));
  ASSERT_ALLOC(u0_tx = (fftw_complex**) fftw_malloc (Ntransl*sizeof(fftw_complex*)));
  ASSERT_ALLOC(u0_ty = (fftw_complex**) fftw_malloc (Ntransl*sizeof(fftw_complex*)));
  ASSERT_ALLOC(plan_u0 = fftw_plan_dft_2d(ny,nx,u0,dft_u0,FFTW_FORWARD,FFTW_ESTIMATE));
  ASSERT_ALLOC(iplan_u0_tx = (fftw_plan*) malloc (Ntransl*sizeof(fftw_plan)));
  ASSERT_ALLOC(iplan_u0_ty = (fftw_plan*) malloc (Ntransl*sizeof(fftw_plan)));
  for(id_t=0;id_t<Ntransl;id_t++) {
    ASSERT_ALLOC(u0_tx[id_t]  = (fftw_complex*) fftw_malloc (Omega*sizeof(fftw_complex)));
    ASSERT_ALLOC(u0_ty[id_t]  = (fftw_complex*) fftw_malloc (Omega*sizeof(fftw_complex)));
    ASSERT_ALLOC(iplan_u0_tx[id_t] = fftw_plan_dft_2d(ny,nx,dft_vt,u0_tx[id_t],FFTW_BACKWARD,FFTW_ESTIMATE));
    ASSERT_ALLOC(iplan_u0_ty[id_t] = fftw_plan_dft_2d(ny,nx,dft_vt,u0_ty[id_t],FFTW_BACKWARD,FFTW_ESTIMATE));
  }

  // initialization //
  fftw_execute(plan_u0);
  for(id_t=0;id_t<Ntransl;id_t++) {
    if (verbose) {
      if (MATLAB_DISPLAY) {
	printf("precompute horizontal/vertical translations of u0: step %d/%d\n",1+id_t,Ntransl);
	fflush(stdout);
      }
      else { printf("\33[2K\rprecompute horizontal/vertical translations of u0: step %d/%d",1+id_t,Ntransl); fflush(stdout); }
    }
    delta = -0.5 + (double)id_t/(double)Ntransl;
    horizontal_translation(dft_vt,dft_u0,nx,ny,delta,iplan_u0_tx[id_t]);
    vertical_translation(dft_vt,dft_u0,nx,ny,delta,iplan_u0_ty[id_t]);
  }

  if (verbose) {
    if (MATLAB_DISPLAY) {
      printf("precompute horizontal/vertical translations of u0: done\n\n");
      fflush(stdout);
    }
    else { printf("\33[2K\rprecompute horizontal/vertical translations of u0: done\n"); fflush(stdout); }
  }
  dir = (nx < ny);
  id_tx = id_ty = 0; // to avoid gcc warnings

  //---------------- MAIN LOOP ----------------//
  for(adr=y=0;y<ny;y++) {
    for(x=0;x<nx;x++,adr++) {
      if (verbose && !(adr % (Omega/100))) {
	if (MATLAB_DISPLAY) {
	  printf("irregular resampling procedure: in progress %d%%\n",(int)round(100.*(double)adr/(double)Omega));
	  fflush(stdout);
	}
	else { printf("\33[2K\rirregular resampling procedure: in progress %d%%",(int)round(100.*(double)adr/(double)Omega)); fflush(stdout); }
      }
      find_displacement(&id_tx,&id_ty,x,y,K,Ntransl,nx,ny,u0_tx,u0_ty);
      compute_v0(v0,x,y,id_tx,id_ty,u0_tx[id_tx],u0_ty[id_ty],nx,ny,Ntransl,dir);
      if(T) {
	T[adr][0] = -0.5 + (double)id_tx/(double)Ntransl;
	T[adr][1] = -0.5 + (double)id_ty/(double)Ntransl;
      }
    }
  }
  if (verbose) {
    if (MATLAB_DISPLAY) {
      printf("irregular resampling procedure: done\n\n");
      fflush(stdout);
    }
    else { printf("\33[2K\rirregular resampling procedure: done\n\n"); fflush(stdout); }
  }

  // free memory //
  fftw_free(dft_u0);
  fftw_destroy_plan(plan_u0);
  fftw_free(dft_vt);
  for(id_t=0;id_t<Ntransl;id_t++) {
    fftw_free(u0_tx[id_t]);
    fftw_free(u0_ty[id_t]);
    fftw_destroy_plan(iplan_u0_tx[id_t]);
    fftw_destroy_plan(iplan_u0_ty[id_t]);
  }
  fftw_free(u0_tx);
  fftw_free(u0_ty);
  free(iplan_u0_tx);
  free(iplan_u0_ty);

}
