/* @(#)wa_direct.c	17.1.1.1 (ES0-DMD) 01/25/02 17:21:40 */
/*===========================================================================
  Copyright (C) 1995 European Southern Observatory (ESO)
 
  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 2 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, write to the Free 
  Software Foundation, Inc., 675 Massachusetss Ave, Cambridge, 
  MA 02139, USA.
 
  Corresponding concerning ESO-MIDAS should be addressed as follows:
	Internet e-mail: midas@eso.org
	Postal address: European Southern Observatory
			Data Management Division 
			Karl-Schwarzschild-Strasse 2
			D 85748 Garching bei Muenchen 
			GERMANY
===========================================================================*/

/******************************************************************************
**            Copyright (C) 1993 by European Southern Observatory
*******************************************************************************
**
**    UNIT
**
**    Version: 17.1.1.1
**
**    Author: Jean-Luc Starck
**
**    Date:  02/01/25
**    
**    File:  wa_direct.c
**
*******************************************************************************
**
**    DESCRIPTION deconvolve an image with a regularization in the wavelet
**    ----------- space by the van-cittert method
**                 
**
**    PARAMETERS    
**    ----------
** Imag: (keyword: IN_A): input image
**
** PSF: (keyword: IN_B): input PSF
**
** Object: (keyword: OUT_A): output deconvolved object
**
** Nbr_Plan:  (keyword: INPUTI[1]): number of scales
**           it is not necessary to take values for Nbr_Plan superior to 5
**          
** Tab_Gamma: (keyword: INPUTR[1..Nbr_Plan-1]): 
**           regularization parameters for all the scales
**
**    RESULTS 
**    ------- 
**   two files are created. The first contains the deconvolved object
**   and the second the residual                 
**            
******************************************************************************/

static char sccsid[] = "@(#)wa_direct.c 17.1.1.1 02/01/25 ESO 1993 @(#)";

#include <stdio.h>
#include <math.h>
#include <string.h>

#include<midas_def.h>

#include "Def_Math.h"
#include "Def_Mem.h"
#include "Def_Wavelet.h"

#define VISU_PARAM TRUE

float pyr_2d_cf_filter();

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

float product_h(u,v,Num, Nl, Nc, Type_Wavelet, Fc)
int u,v;
int Num;
float Fc;
int Type_Wavelet, Nl, Nc;
{
    float Prod = 1., Phi;
    int i, Dep;
    
    for (i = Num; i >= 0; i--)
    {
        INT_POW (2,i,Dep);
        Phi = pyr_2d_cf_filter(FILTER_H,
                                   (float) (Dep * u),
                                   (float) (Dep * v),
                                   Fc,
                                   Nl, Nc,
                                   Type_Wavelet);
        Prod *= Phi;
    }
    return (Prod);
}

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

wavelet_oper1 (Wavelet, Psf, Tab_Gamma, Oper1)
wave_transf_des *Wavelet;
complex_float *Psf;
float *Tab_Gamma, *Oper1;
{
    int i, j, u, v, Dep_Plan, Dep, Nl, Nc, Nbr_Etap;
    float Phi, Psi, Som_Regul, Den;
    float Fc, P2;
    int Type_Wavelet, Ind_Plan;

    Nbr_Etap = Wavelet->Nbr_Plan - 1;
    Nl = Wavelet->Nbr_Ligne;
    Nc = Wavelet->Nbr_Col;
    Type_Wavelet = Wavelet->Type_Wave_Transform;
    Fc = Wavelet->Pyramid.Freq_Coup;

    INT_POW (2,Nbr_Etap-1,Dep_Plan);

    /* calcul du denominateur */
    for (i = 0; i < Nl; i++)
    {
        u = i - Nl / 2;
        for (j = 0; j < Nl; j++)
        {
            v = j - Nl / 2;

            /* calcul de | psf |^2 ==> Val_Psf2*/
            P2 =  pow(Psf[i*Nc+j].re, 2.) + pow(Psf[i*Nc+j].im, 2.);

            /* calcul de phi(2^i nu)^2 */
            Phi = pyr_2d_cf_filter(FILTER_H,
                                   (float) (Dep_Plan * u),
                                   (float) (Dep_Plan * v),
                                   Fc,
                                   Nl, Nc,
                                   Type_Wavelet);
            Phi *= product_h(u, v, Nbr_Etap-2, Nl, Nc, Type_Wavelet, Fc);
            Den = Phi * Phi;
            Som_Regul = 0.;
            /* calcul de som_i psi(2^i nu)^2 */
            for (Ind_Plan = 1; Ind_Plan <= Nbr_Etap; Ind_Plan++)
            {
                INT_POW (2,Ind_Plan-1,Dep);
                Psi = pyr_2d_cf_filter(FILTER_G,
                                       (float) (Dep * u),
                                       (float) (Dep * v),
                                       Fc,
                                       Nl, Nc,
                                       Type_Wavelet);
                Psi *= product_h(u, v, Ind_Plan-2, Nl, Nc, Type_Wavelet, Fc);
                Som_Regul += Psi*Psi*Tab_Gamma[Ind_Plan-1];
                Den += Psi*Psi;
            }
            Oper1 [i*Nc+j] = Den * P2 + Som_Regul;
        }
    }
}

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

static calcul_etap2 (Tab_Coeff, Pict_Fft, Pict_UV,  
                     Nl1, Nc1, Num_Etap, Wavelet)
int Tab_Coeff;
complex_float *Pict_Fft,*Pict_UV;
int Nl1,Nc1,Num_Etap;
wave_transf_des *Wavelet;
{
    int i,j,Dep,Nl,Nc, Type_Wavelet; 
    int ind_pict_uv,ind_pict_fft;
    int Nl_2, Nc_2,Nl1_2, Nc1_2;
    int u,v,u1,v1;
    float Coef, Fc;

    Nl = Wavelet->Nbr_Ligne;
    Nc = Wavelet->Nbr_Col;
    Type_Wavelet = Wavelet->Type_Wave_Transform;
    Fc = Wavelet->Pyramid.Freq_Coup;

    Nl1_2 = Nl1 / 2;
    Nc1_2 = Nc1 / 2;
    Nl_2 = Nl / 2;
    Nc_2 = Nc / 2;

    INT_POW (2,Num_Etap-1,Dep);

    for (i = 0; i < Nl1; i++)
    {
        u = (i - Nl1_2);
        u1 = Dep * u;
        for (j = 0; j < Nc1; j++)
        {
            v = (j - Nc1_2);
            ind_pict_uv = (u + Nl_2) * Nc + v + Nc_2;
            ind_pict_fft = i * Nc1 + j;
            v1 = Dep * v;

            Coef = pyr_2d_cf_filter(Tab_Coeff, (float) u1, (float) v1,
                                    Fc, Nl, Nc, Type_Wavelet);
            Coef *= product_h(u, v, Num_Etap-2, Nl, Nc, Type_Wavelet, Fc);
            Pict_UV [ind_pict_uv].re += Pict_Fft [ind_pict_fft].re * Coef;
            Pict_UV [ind_pict_uv].im += Pict_Fft [ind_pict_fft].im * Coef;
        }
    }
}

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

wavelet_oper2 (Wavelet, Psf, Oper2)
wave_transf_des *Wavelet;
complex_float *Psf;
complex_float *Oper2;
{
    int i, j, Nl, Nc;
    complex_float Val_Psf, Val_Psf2;
    float *Plan;
    complex_float *Plan_cf;
    int Nl_p,Nc_p;

    Nl = Wavelet->Nbr_Ligne;
    Nc = Wavelet->Nbr_Col;

    for (i = 0; i < Nl*Nc; i++) Oper2[i].re = Oper2[i].im = 0.;

    for (i = 1; i <= Wavelet->Nbr_Plan; i++)
    {
        wavelet_extract_plan (Wavelet, &Plan, &Nl_p, &Nc_p, i);
        Plan_cf = cf_vector_alloc (Nl_p*Nc_p);
        for (j = 0; j < Nl_p*Nc_p; j++) 
                           Plan[j] *= (float)(Nl * Nc) / (float)(Nl_p * Nc_p);
        prepare_fft_real (Plan, Plan_cf, Nl_p);
        ft_cf_any_power_of_2 (Plan_cf, 1, Nl_p);

        if (i != Wavelet->Nbr_Plan)
              calcul_etap2 (FILTER_G, Plan_cf, Oper2, Nl_p, Nc_p, i, Wavelet);
        else  calcul_etap2 (FILTER_H, Plan_cf, Oper2, Nl_p,Nc_p, i-1, Wavelet);
        free ((char *) Plan);
        free ((char *) Plan_cf);
    }

    for (i = 0; i < Nl; i++)
    {
        for (j = 0; j < Nl; j++)
        {
            Val_Psf.re = Psf[i*Nc+j].re;
            Val_Psf.im = -Psf[i*Nc+j].im;
            CF_MLT(Val_Psf, Oper2[i*Nc+j], Val_Psf2);
            Oper2[i*Nc+j].re = Val_Psf2.re;
            Oper2[i*Nc+j].im = Val_Psf2.im;
        }
    }
}

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

wa_dec_direct (Imag, Psf, Result, Nl, Nc, Nbr_Plan, Tab_Gamma)
float *Imag, *Psf, *Result;
int Nl,Nc,Nbr_Plan;
float *Tab_Gamma;
{
    complex_float *Psf_cf;
    wave_transf_des Wavelet;
    int i, Type_Transform = 6;
    float Fc = 0.5, *Oper1;
    complex_float *Oper2;

    wavelet_transform_data (Imag, Nl, Nc, &Wavelet, 
                            Type_Transform, Fc, Nbr_Plan);

    Psf_cf = cf_vector_alloc (Nl*Nc);
    prepare_fft_real (Psf, Psf_cf, Nl);
    ft_cf_any_power_of_2 (Psf_cf, 1, Nl);

    Oper1 = f_vector_alloc (Nl*Nc);
    wavelet_oper1 (&Wavelet, Psf_cf, Tab_Gamma, Oper1);

    Oper2 = cf_vector_alloc (Nl*Nc);
    wavelet_oper2 (&Wavelet, Psf_cf, Oper2);

    for (i = 0; i < Nl*Nc; i++)
    {
        if (fabs(Oper1[i]) > FLOAT_EPSILON)
        {
            Oper2[i].re /= Oper1[i];
            Oper2[i].im /= Oper1[i];
        }
        else  Oper2[i].re = Oper2[i].im = 0.;
    }
    ft_cf_any_power_of_2 (Oper2, -1, Nl);

    /* Sauvegarde du resultat */
    for (i = 0; i < Nl*Nc; i++) Result[i] = Oper2[i].re;

    wave_io_free (&Wavelet);
    free ((char *) Psf_cf);
    free ((char *) Oper1);
    free ((char *) Oper2);
}

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

main()
{
   float *Tab_Gamma;
   int i,Nl,Nc,Nl_Psf,Nc_Psf,Nl1,Nc1;
   int Nbr_Plan;
   string Name_File_Imag, Name_File_Psf, Name_File_Out;
   float *Psf, *Imag, *Imag1, *Psf1, *Result;
   int Unit;
   int Null, Actvals, Buffer_Int, Maxvals, Felem;
   int Stat;

    /* Initialisation */  
    SCSPRO("wa_direct");

    /* read the image 1 file name */
    Felem = 1;
    Maxvals = 60;
    Stat = SCKGETC("IN_A", Felem, Maxvals, &Actvals, Name_File_Imag);

    /* read the PSF file name */
    Stat = SCKGETC("IN_B", Felem, Maxvals, &Actvals, Name_File_Psf);

    /* read the output file name */
    Stat = SCKGETC("OUT_A", Felem, Maxvals, &Actvals, Name_File_Out);

    /* read the number of scales */
    Maxvals = 1;
    Stat = SCKRDI("INPUTI", Felem, Maxvals, &Actvals, 
                            &Buffer_Int, &Unit, &Null);
    Nbr_Plan = Buffer_Int;

    Tab_Gamma = f_vector_alloc (Nbr_Plan-1);
    for (i = 0; i < Nbr_Plan-1; i++)
    {
       float Val;

       Felem = i+1;
       Maxvals = 1;
       Stat = SCKRDR("INPUTR", Felem, Maxvals, &Actvals, &Val, &Unit, &Null);
       Tab_Gamma[i] = Val;
    }

    /* read the input images */
    io_read_file_to_pict_f (Name_File_Imag, &Imag, &Nl, &Nc);
    io_read_file_to_pict_f (Name_File_Psf, &Psf, &Nl_Psf, &Nc_Psf);

#if VISU_PARAM
   { char Send[100];
    sprintf(Send," File_Name_Imag = %s\n", Name_File_Imag);
    SCTPUT(Send);
    sprintf(Send," File_Name_Psf = %s\n", Name_File_Psf);
    SCTPUT(Send);
    sprintf(Send," File_Name_Out = %s\n", Name_File_Out);
    SCTPUT(Send);
    sprintf(Send," Nbr_Plan = %d\n", Nbr_Plan);
    SCTPUT(Send);
    for (i = 0; i < Nbr_Plan-1; i++) {
           printf ("Gamma[%d] = %f\n", i+1, Tab_Gamma[i]);
           SCTPUT(Send);}
   }
#endif

    /* the image is inserted in a bigger square image
       with a size which is a power of two
    */
    dec_line_column (Nl, &Nl1);
    dec_line_column (Nc, &Nc1);
    if (Nl1 < Nc1) Nl1 = Nc1;
    else Nc1 = Nl1;
#if VISU_PARAM
    printf ("Image Size: %d\n", Nl1);
#endif
    Imag1 = f_vector_alloc (Nl1*Nc1);
    dec_insert_ima (Imag, Nl, Nc, Imag1, Nl1, Nc1);

    /* Compute the FFT of the PSF in an image with the same size */
    Psf1 = f_vector_alloc (Nl1*Nc1);
    dec_center_psf (Psf, Nl_Psf, Nc_Psf, Psf1, Nl1, Nc1);
    NORM_ENERG(Psf1, Nl1*Nc1);
    free ((char *) Psf);

    Result = f_vector_alloc (Nl1*Nc1);
    wa_dec_direct (Imag1, Psf1, Result, Nl1, Nc1, Nbr_Plan, Tab_Gamma);

    dec_extract_ima (Result, Nl1, Nc1, Imag, Nl, Nc);
    io_write_pict_f_to_file (Name_File_Out, Imag, Nl, Nc);

    free ((char *) Imag);
    free ((char *) Imag1);
    free ((char *) Result);
    free ((char *) Psf1);
    SCSEPI();
}

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