/****************************************************************************
 * Copyright (c) 2007 Einir Valdimarsson and Chrysanthe Preza
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 ****************************************************************************/

#ifndef TOOL_MGR_H
#define TOOL_MGR_H

#include "wu/wuImage.h"
#include "wu/wuHeader.h"
#include "blitz/arrayManip.h"
#include "blitz/fftwInterface.h"
#include <blitz/array.h>

#include <complex>

#include <string>
#include "alglib/svd.h"

namespace cosm {

class wuHeader;

class ToolMgr {

public:
    ToolMgr( 
        const std::string& inName, 
        const std::string& outName = "" 
    );
    ToolMgr( 
        const std::string& inName, 
        const std::string& refName, 
        const std::string& outName 
    );
    ~ToolMgr();

    bool info( wuHeader& header );

    bool addHeader( 
        int xDim, 
        int yDim, 
        int zDim, 
        wuHeader::type type, 
        bool endian 
    );

    bool removeHeader( int );

    bool convert( wuHeader::type type, int );

    bool transformation( 
        int xDim, 
        int yDim, 
        int zDim, 
        int xShift, 
        int yShift, 
        int zShift, 
        double value  
    );

    bool resample( 
        int xDim, 
        int yDim, 
        int zDim, 
        bool up 
    );

    bool compare( 
        double& maximum, 
        double& mean, 
        double& meanSquare 
    ); 

    bool convolve(
    		unsigned short size = 0,
    		bool centeredPsf = true
    		);

    bool shift(
        int xShift,
        int yShift,
        int zShift,
        bool circular
    );
 
    bool scale(
        bool sumFlag = false,
        bool maxFlag = false,
        double value = 1.0
    );

    bool create( 
        int xDim, 
        int yDim, 
        int zDim, 
        wuHeader::type type,
        double value 
    );

    bool ellipsoid( 
        int xCenter, 
        int yCenter, 
        int zCenter, 
        int xRadius, 
        int yRadius, 
        int zRadius, 
        double value  
    );

    bool box( 
        int xCenter, 
        int yCenter, 
        int zCenter, 
        int xRadius, 
        int yRadius, 
        int zRadius, 
        double value  
    );

    bool point( 
        int xCenter, 
        int yCenter, 
        int zCenter, 
        double value  
    );

    bool variant(
        int number,
        int start,
        int size,
    	bool centeredPsf = true,
        bool strataModel = true
    );

    bool pca(
        int numberOfPsf,
        int depthOfPsf,
        int numberOfComponent
    );

    bool cpm(
    		bool centeredPsf = true
    );

protected:

    template<typename T, int  N>
    Array<T,N> variantImage(
        int numberOfStrata,
        int startOfStrata,
        int sizeOfStrata,
        bool centeredPsf
    );

    template<typename T, int  N>
    Array<T,N> variantImagePCA(
        int numberOfComponents,
        int startOfPCAPSF,
        int numberOfPSF,
        bool centeredPsf
    );

	template<typename T, int  N>
	bool pcaImage(
        int numberOfPsf,
        int depthOfPsf,
        int numberOfComponent
		);

    template<typename T, int N>
    Array<T,N> CPMWithFFT(
	    bool centeredPsf
    );

    template<typename T, int N>
    Array<T,N> IMRESIZE(
	    Array<T,N> MaskFile,
        int CPMSize
    );
    
private:
     
    ToolMgr( const ToolMgr& );                    // not allowed
    const ToolMgr& operator()( const ToolMgr& );  // not allowed

private:

    std::string inName_;
    std::string outName_;
    std::string refName_;

};


template<typename T, int  N>
Array<T,N> ToolMgr::variantImage(
    int numberOfStrata,
    int startOfStrata,
    int sizeOfStrata,
    bool centeredPsf
) {
    // read the object image
    Array<T, N> img;
    wuDataRead(img, inName_);

    // find the psf prefix and suffix
    std::string psfname = refName_;
    std::string psfsuffix;
    std::string psfprefix;
    size_t i = psfname.rfind('.', psfname.length());
    if ( i != std::string::npos )
    {
        psfprefix = psfname.substr(0,i);
        psfsuffix = psfname.substr(i, psfname.length());
    }

    Array<RectDomain<N>, 1> strata(numberOfStrata+2);
    Array<Array<T,N>, 1> psfs(numberOfStrata+1);

    for ( int m = 0; m <= numberOfStrata+1; m++ )
    {
        TinyVector<int, N> l(img.lbound());
        TinyVector<int,N> u(img.ubound());
        if( m == 0 )
        {
            u(0) = startOfStrata - 1;
        }
        else if ( m <= numberOfStrata )
        {
            l(0) = startOfStrata + sizeOfStrata * (m-1);
            u(0) = startOfStrata + sizeOfStrata * m - 1;
        }
        else
        {
            l(0) = startOfStrata + sizeOfStrata * (m-1);
        }
        strata(m).setlbound(l);
        strata(m).setubound(u);
        std::cout <<"Strata: "<<m<<" "<<l<<", "<<u<<std::endl;

        if ( m <= numberOfStrata )
        {
            std::ostringstream psfm;
            psfm << psfprefix << m << psfsuffix;
            std::cout <<psfm.str()<<std::endl;
            wuDataRead(psfs(m), psfm.str());
        }
    }

    // compute the interpolation constants
    Array<T,1> a;
    a.resize(img.extent(0));
    a = 0;
    int index = strata(0).lbound(int(0));
    for ( int m = 0; m < strata.length(0); m++ ) 
    {
        int l = strata(m).lbound(int(0));
        int u = strata(m).ubound(int(0));
        int size = u - l + 1;
        for ( int j = 0; j < size; j++ ) 
        { 
            a(index) = T(1) - T(j)/T(size);
            index++;
        }
    }
    std::cout <<"a: "<< a << std::endl;

    TinyVector<int,N> extent(img.extent());
    Array<T,N> s;              
    Array<T,N> s1;             
    s.resize(extent);
    s1.resize(extent);
    extent(N-1) = extent(N-1)/2+1;

    // resize the psfs and compute the Fourier transform (OTF)
    Array<Array<std::complex<T>,N>, 1> psfsF;  
    psfsF.resize(psfs.length(0));
    for ( int m = 0; m < psfs.length(0); m++ ) 
    {
        psfsF(m).resize(extent);
        Array<T,N> psfResized(img.extent()); 
		//Version 0.8.11::External-3
        //Process Psf file based on the choice of user that whether it's centered
        if (centeredPsf){
        	padAround(psfs(m), psfResized);
        }else{
        	padCenter(psfs(m), psfResized);
        }
        psfsF(m) = forwardFFT(psfResized);
    }

    // compute the image
    fftwInterface<T,N> fftw;
    Array<std::complex<T>,N> imgF;  
    Array<std::complex<T>,N> sF;  

    imgF.resize(extent);
    sF.resize(extent);

    int m;
    // get convolution of img with interpolation of psfs 
    // (multiplication and in Fourier domain)
    imgF = 0;
    int size = strata.length(0) - 2;
    for ( m = 0; m < size; m++ ) 
    {
        s = 0;
        if ( m == 0 )
        {
            s(strata(m)) = img(strata(m));
        }
        else if ( m == size - 1 )
        {
            s(strata(m+2)) = img(strata(m+2));
        }
        s(strata(m+1)) = img(strata(m+1));

        s1 = s;
        multiplyStratum(strata(m+1), s, a, true);
        multiplyStratum(strata(m+1), s1, a, false);

        fftw.plan(s, sF);
        fftw.execute();
        imgF += sF * psfsF(m);

        fftw.plan(s1, sF);
        fftw.execute();
        imgF += sF * psfsF(m+1);
    }
    // convert back to time domain
    fftw.plan(imgF, img);
    fftw.execute();
    img /= img.size();

	//Version 0.8.11::External-3
    //Process Psf file based on the choice of user that whether it's centered
    if (centeredPsf){
		TinyVector<int,3> shift((img.shape()+1)/2);
		Array<T,3> res = circularShift(img, shift);

		return res;
    }else{
        return img;

    }
}

template<typename T, int  N>
Array<T,N> ToolMgr::variantImagePCA(
    int numberOfComponents,
    int startOfPCAPSF,
    int numberOfPSF,
    bool centeredPsf
) {

    // read the object image
    Array<T, N> img;
    wuDataRead(img, inName_);

    // find the psf prefix and suffix
    std::string psfname = refName_;
    std::string psfsuffix;
    std::string psfprefix;
    size_t i = psfname.rfind('.', psfname.length());
    if ( i != std::string::npos )
    {
        psfprefix = psfname.substr(0,i);
        psfsuffix = psfname.substr(i, psfname.length());
    }

    //Read PCA coefficient and check whether existing number of PSFs is not less than request
    Array<T, 2> coefficient;

    std::ostringstream psfCoefficient;
    psfCoefficient << psfprefix << "_PCA_Coefficient" << psfsuffix;
    std::cout <<"read PCA coefficient from " << psfCoefficient.str()<<std::endl;

    wuDataRead(coefficient, psfCoefficient.str());

    //Read Psfs 
    Array<Array<T, N>, 1> psfs(numberOfComponents+1);

    //Read PCA_BASE_ZERO
    std::ostringstream psf_base_zero;
    psf_base_zero <<psfprefix << "_PCA_BaseZero"<<psfsuffix;
    std::cout <<psf_base_zero.str()<<std::endl;
    wuDataRead(psfs(0), psf_base_zero.str());

    //Read PCA_BASE_PSFs
    for ( int m = 0; m < numberOfComponents; m++ )
    {
        std::ostringstream psfm;
        psfm << psfprefix << m << psfsuffix;
        std::cout <<psfm.str()<<std::endl;
        wuDataRead(psfs(m+1), psfm.str());
    }

    //Organize coefficient of PCA
    Array<Array<T,1>, 1> a(numberOfComponents+1);
    for (int m = 0; m < numberOfComponents + 1; m++)
    {
        a(m).resize(img.extent(0));
        if (m == 0){
            //For PCA_BASE_ZERO
            a(m) = 1;
        } else {
            // For PCA_BASE_PSFs
            //Only the coefficient which index is in scope of numberOfPSF are assigned dynamically
            //Rest lower or higher parts are assigned by fixed bounding coefficient
            for (int i = 0; i < img.extent(0); i++)
            {
                //Lower
                if (i < startOfPCAPSF)
                    a(m)(i) = coefficient(m-1, 0);
                //Or higher
                else if (i >= startOfPCAPSF + numberOfPSF)
                    a(m)(i) = coefficient(m-1, numberOfPSF - 1);
                else
                    a(m)(i) = coefficient(m-1, i-startOfPCAPSF);
            }

        }
    }

    std::cout <<"a: "<< a << std::endl;

    // resize the psfs and compute the Fourier transform (OTF)
    TinyVector<int,N> extent(img.extent());
    Array<T,N> s;              
    Array<T,N> s1;             
    s.resize(extent);
    s1.resize(extent);
    extent(N-1) = extent(N-1)/2+1;

    Array<Array<std::complex<T>,N>, 1> psfsF;  
    psfsF.resize(psfs.length(0));
    
    for ( int m = 0; m < psfs.length(0); m++ ) 
    {
        psfsF(m).resize(extent);
        Array<T,N> psfResized(img.extent()); 
		//Version 0.8.11::External-3
        //Process Psf file based on the choice of user that whether it's centered
        if (centeredPsf){
            padAround(psfs(m), psfResized);
        }else{
        	padCenter(psfs(m), psfResized);
        } 
        psfsF(m) = forwardFFT(psfResized);
    }

    // compute the image
    fftwInterface<T,N> fftw;
    Array<std::complex<T>,N> imgF;  
    Array<std::complex<T>,N> sF;  

    imgF.resize(extent);
    sF.resize(extent);

    // get convolution of img with interpolation of psfs 
    // (multiplication and in Fourier domain)
    imgF = 0;
    
    for (int m = 0; m < psfs.length(0); m++ )
    {
        s = img;
        for (int n = 0; n < img.extent(0); n++)
        {
            //s(n)* = a(m)(n);
            for (int ind_col = 0; ind_col< img.extent(1); ind_col++)
                for (int ind_depth = 0; ind_depth< img.extent(2); ind_depth++)
                    s(n, ind_col, ind_depth) *= a(m)(n);
        }
        fftw.plan(s, sF);
        fftw.execute();
        imgF += sF * psfsF(m);
    }

    // convert back to time domain
    fftw.plan(imgF, img);
    fftw.execute();
    img /= img.size();

	//Version 0.8.11::External-3
    //Process Psf file based on the choice of user that whether it's centered
    if (centeredPsf){
		TinyVector<int,N> shift((img.shape()+1)/2);
		Array<T,N> res = circularShift(img, shift);

		return res;
    }else{
        return img;

    }
}

template<typename T, int  N>
bool ToolMgr::pcaImage(
        int numberOfPsf,
        int depthOfInterval,
        int numberOfComponent
		){

	int Size_PSF, depthOfPsf;

	//Import original Psf(s)
	// Read original Psf(s)
	// find the psf prefix and suffix
    std::string psfname = inName_;
    std::string psfsuffix;
    std::string psfprefix;
    size_t i = psfname.rfind('.', psfname.length());
    if ( i != std::string::npos )
    {
        psfprefix = psfname.substr(0,i);
        psfsuffix = psfname.substr(i, psfname.length());
    }else
	{
        psfprefix = psfname;
        psfsuffix = "";
	}

	// 'Size_PSF' and 'depthOfPsf' are recieved from psf itself
	WUImage tmp_psf;
	if ( !tmp_psf.ReadData(psfprefix + "0" + psfsuffix) ) 
    {
        return false;
    }
	wuHeader header;
    header.read(psfprefix + "0" + psfsuffix);

	Size_PSF = header.rows();
	std::cout << " Size_PSF is "<< Size_PSF <<std::endl;

	depthOfPsf = header.sections();

	std::cout << " depthOfPsf is "<< depthOfPsf <<std::endl;

	// Read all Psf(s)s

    Array<T,3> tmp_psfs(Size_PSF, Size_PSF, depthOfPsf);
    Array<T,3> tmp_psfs_matlab(depthOfPsf, Size_PSF, Size_PSF);
    //Array<T,4> psfs_4d(Size_PSF, Size_PSF, depthOfPsf,numberOfPsf);

 // Ver. 0.9.18 Nur
    Array<T,4> psfs_4d1(Size_PSF, Size_PSF, depthOfPsf,numberOfPsf/2);
    Array<T,4> psfs_4d2(Size_PSF, Size_PSF, depthOfPsf,numberOfPsf/2);

 // Ver. 0.9.18 Nur
	for (int m=0; m< numberOfPsf/2; m++)
	{
	    std::ostringstream psfm;
        psfm << psfprefix << m << psfsuffix;
        std::cout <<psfm.str()<<std::endl;

		wuDataRead(tmp_psfs_matlab, psfm.str());

        // Transform matlab format to C++ format
        for (int i = 0; i < depthOfPsf; i ++)
            tmp_psfs(Range::all(), Range::all(), i) = tmp_psfs_matlab(i, Range::all(), Range::all());

        psfs_4d1(Range::all(), Range::all(), Range::all(), m) = tmp_psfs;
	}
    
 //Ver. 0.9.18 nur
       int m2 = 0;
	for (int m=0; m< numberOfPsf/2; m++)
	{
          m2 = m + numberOfPsf/2;
        
	    std::ostringstream psfm;
        psfm << psfprefix << m2 << psfsuffix;
        std::cout <<psfm.str()<<std::endl;

		wuDataRead(tmp_psfs_matlab, psfm.str());

        // Transform matlab format to C++ format
        for (int i = 0; i < depthOfPsf; i ++)
            tmp_psfs(Range::all(), Range::all(), i) = tmp_psfs_matlab(i, Range::all(), Range::all());

        psfs_4d2(Range::all(), Range::all(), Range::all(), m) = tmp_psfs;
	}
    

	Array<T, 3> psf_3d_subset(Size_PSF, Size_PSF, numberOfPsf);
	Array<T, 2> psf_2d_subset(Size_PSF*Size_PSF, numberOfPsf);
	Array<T, 2> psf_2d_mean(Size_PSF, Size_PSF);
	Array<T, 2> UU(Size_PSF*Size_PSF, numberOfPsf);
	Array<T, 4> UU_image(Size_PSF, Size_PSF, depthOfPsf, numberOfComponent);
	Array<T, 3> CC(depthOfPsf, numberOfComponent, numberOfPsf);

	Array<T, 2> tmp_2d_mean_dev(Size_PSF, Size_PSF);
	Array<T, 2> tmp_2d_UU(Size_PSF, Size_PSF);
	Array<T, 2> tmp_2d_CC(Size_PSF, Size_PSF);

	//Step 1/3
	// Compute 2D PCA components once
	for (int ii = 0; ii < depthOfPsf; ii++)
	{
		std::cout << " depthOfPsf: "<< ii <<std::endl;

		for (int indPsf = 0; indPsf < numberOfPsf; indPsf++)
		{
			//squeeze 4D to 3D
         		//psf_3d_subset(Range::all(), Range::all(), indPsf) = psfs_4d(Range::all(), Range::all(), ii, indPsf); //Ver. 0.9.17
                     //Ver. 0.9.18
                     if (indPsf < numberOfPsf/2){
                         psf_3d_subset(Range::all(), Range::all(), indPsf) = psfs_4d1(Range::all(), Range::all(), ii, indPsf); //Ver. 0.9.17
                     } else{
                         psf_3d_subset(Range::all(), Range::all(), indPsf) = psfs_4d2(Range::all(), Range::all(), ii, indPsf - numberOfPsf/2); //Ver. 0.9.17
                     }

			//reshape 3D data to 2D
			for (int indPsfSize = 0; indPsfSize < Size_PSF; indPsfSize ++){
				psf_2d_subset(Range(indPsfSize*Size_PSF, (indPsfSize + 1)*Size_PSF - 1), Range(indPsf,indPsf))
					= psf_3d_subset(indPsfSize, Range::all(), Range(indPsf,indPsf));
			}
		}

		//processing SVD function
		// Parameters of SVD function
		
//    W           -   contains singular values in descending order.
//    U           -   if UNeeded=0, U isn't changed, the left singular vectors
//                   are not calculated.
//                    if Uneeded=1, U contains left singular vectors (first
//                    min(M,N) columns of matrix U). Array whose indexes range
//                    within [0..M-1, 0..Min(M,N)-1].
//                    if UNeeded=2, U contains matrix U wholly. Array whose
//                   indexes range within [0..M-1, 0..M-1].
//    VT          -   if VTNeeded=0, VT isn’t changed, the right singular vectors
//                    are not calculated.
//                    if VTNeeded=1, VT contains right singular vectors (first
//                    min(M,N) rows of matrix V^T). Array whose indexes range
//                    within [0..min(M,N)-1, 0..N-1].
//                    if VTNeeded=2, VT contains matrix V^T wholly. Array whose
//                    indexes range within [0..N-1, 0..N-1].
		
		int M = Size_PSF*Size_PSF, Nx = numberOfPsf, UNeeded = 1, VTNeeded = 0, AdditionalMemory = 2;

		// Data are usd for formating translation
		ap::real_2d_array ap_psf_2d_subset;
		ap_psf_2d_subset.setbounds( 0, M - 1, 0, Nx - 1 );
		ap::real_1d_array ap_w;
		ap::real_2d_array ap_u;
		ap::real_2d_array ap_vt;

		// 1. Translating target from blitz::Array to ap::real_2d_array
		for (int i = 0; i< M; i++)
		{
			for (int j = 0; j<Nx; j++)
			{
				ap_psf_2d_subset(i, j) = psf_2d_subset(i, j);
			}
		}

		//2. Performing SVD function
		rmatrixsvd(ap_psf_2d_subset, M, Nx, UNeeded, VTNeeded, AdditionalMemory, ap_w, ap_u, ap_vt);

		//3. Translating result from ap::real_2d_array to blitz::Array
		for (int i = 0; i<M; i++)
		{
			//std::cout << "Line " << i <<": ";
			for (int j = 0; j<Nx; j++)
			{
				UU(i, j) = ap_u(i, j);
				//std::cout << UU(i, j) << " ";
			}
			//std::cout<<std::endl;
		}

		//Compute mean
		firstIndex firstI;
		secondIndex secondI;
		thirdIndex thirdI;

		psf_2d_mean = mean(psf_3d_subset(firstI, secondI, thirdI), thirdI);

		// COmpute variables of CC
		// through each component
		for (int jj = 0; jj< numberOfComponent; jj++)
		{
			//reshape 1D to 2D
			for (int indPsfSize = 0; indPsfSize < Size_PSF; indPsfSize ++){
				UU_image(indPsfSize, Range::all(), ii, jj) = UU(Range(indPsfSize*Size_PSF, (indPsfSize + 1)*Size_PSF - 1), jj);
			}

			// through each Psf
			for (int iii = 0; iii < numberOfPsf; iii++)
			{
				tmp_2d_mean_dev = psf_3d_subset(Range::all(), Range::all(), iii) - psf_2d_mean;
				tmp_2d_UU = UU_image(Range::all(), Range::all(), ii, jj);
				for (int i_Ind = 0; i_Ind < Size_PSF; i_Ind++)
					for (int j_Ind = 0; j_Ind < Size_PSF; j_Ind++)
						tmp_2d_CC(i_Ind, j_Ind) = tmp_2d_mean_dev(i_Ind, j_Ind)* tmp_2d_UU(i_Ind, j_Ind);
				CC(ii, jj, iii) = sum (tmp_2d_CC);
			}
		}

	}//end of ii=1:Nz

	//Step 2/3:
	//Compute 2D PCA components twice (finally)
	Array<T, 3> psf_final_3d_subset(depthOfPsf, numberOfComponent, numberOfPsf);
	Array<T, 2> psf_final_2d_subset(depthOfPsf*numberOfComponent, numberOfPsf);
	//Array<T, 2> psf_2d_mean(Size_PSF, Size_PSF);
	Array<T, 2> UU_final(depthOfPsf*numberOfComponent, numberOfPsf);
	Array<T, 3> UU_final_image(depthOfPsf, numberOfComponent, numberOfComponent);
	Array<T, 2> CC2(numberOfComponent, numberOfPsf);

	Array<T, 2> tmp_final_2d_squeeze(depthOfPsf, numberOfComponent);
	Array<T, 2> tmp_final_2d_UU(depthOfPsf, numberOfComponent);
	Array<T, 2> tmp_final_2d_CC(depthOfPsf, numberOfComponent);

	psf_final_3d_subset = CC;

	for (int kk = 0; kk < numberOfPsf; kk++)
	{
		//reshape 3D to 2D
		for (int IndDepth = 0; IndDepth < depthOfPsf; IndDepth++)
			psf_final_2d_subset(Range(IndDepth*numberOfComponent, (IndDepth+1)*numberOfComponent - 1), Range(kk, kk)) 
			= psf_final_3d_subset(IndDepth, Range::all(), Range(kk, kk));
	}

	//processing SVD function
	// Parameters of SVD function
	
	int final_M =depthOfPsf*numberOfComponent, final_N = numberOfPsf, UNeeded = 1, VTNeeded = 0, AdditionalMemory = 2;

	// Data are usd for formating translation
	ap::real_2d_array ap_final_psf_2d_subset;
	ap_final_psf_2d_subset.setbounds( 0, final_M - 1, 0, final_N - 1 );
	ap::real_1d_array ap_final_w;
	ap::real_2d_array ap_final_u;
	ap::real_2d_array ap_final_vt;

	// 1. Translating target from blitz::Array to ap::real_2d_array
	for (int i = 0; i< final_M; i++)
	{
		for (int j = 0; j<final_N; j++)
		{
			ap_final_psf_2d_subset(i, j) = psf_final_2d_subset(i, j);
		}
	}

	//2. Performing SVD function
	rmatrixsvd(ap_final_psf_2d_subset, final_M, final_N, UNeeded, VTNeeded, AdditionalMemory, ap_final_w, ap_final_u, ap_final_vt);

	//3. Translating result from ap::real_2d_array to blitz::Array
	for (int i = 0; i<final_M; i++)
	{
		//std::cout << "Line " << i <<": ";
		for (int j = 0; j<final_N; j++)
		{
			UU_final(i, j) = ap_final_u(i, j);
			//std::cout << UU_final(i, j) << " ";
		}
		//std::cout<<std::endl;
	}

	// Compute variables of CC
	// Through each component
	for (int jj = 0; jj < numberOfComponent; jj ++)
	{
		//reshape 1D to 2D
		for (int IndDepth = 0; IndDepth < depthOfPsf; IndDepth ++){
				UU_final_image(IndDepth, Range::all(), jj) 
					= UU_final(Range(IndDepth*numberOfComponent, (IndDepth + 1)*numberOfComponent - 1), jj);
		}

		// Through each psf
		for (int iii = 0; iii < numberOfPsf; iii++)
		{
			tmp_final_2d_squeeze = psf_final_3d_subset(Range::all(), Range::all(), iii);
			tmp_final_2d_UU = UU_final_image(Range::all(), Range::all(), jj);

			for (int i_Ind = 0; i_Ind < depthOfPsf; i_Ind++)
					for (int j_Ind = 0; j_Ind < numberOfComponent; j_Ind++)
						tmp_final_2d_CC(i_Ind, j_Ind) = tmp_final_2d_squeeze(i_Ind, j_Ind)* tmp_final_2d_UU(i_Ind, j_Ind);
			CC2(jj, iii) = sum (tmp_final_2d_CC);
		}
	}

	// Step 3/3
	//Compute base Psf(s)
	Array<T, 4> Base_PSF(Size_PSF, Size_PSF, depthOfPsf, numberOfComponent);
    Array<T, 3> Base_PSF_matlab(depthOfPsf, Size_PSF, Size_PSF);
	Array<T, 2> tmp_base_psf(Size_PSF, Size_PSF);

	Base_PSF = 0;

	for (int jj = 0; jj < numberOfComponent; jj ++)
		for (int ii = 0; ii < depthOfPsf; ii ++)
			for (int jjj = 0; jjj < numberOfComponent; jjj ++)
			{
				tmp_base_psf = UU_final_image(ii, jjj, jj)*UU_image(Range::all(), Range::all(), ii, jjj);
				Base_PSF(Range::all(), Range::all(), ii, jj) = tmp_base_psf + Base_PSF(Range::all(), Range::all(), ii, jj);
			}

	//Export output data
	// Output data: Represent original Psf(s) proximally by production of two matrix: Base_Psf*CC2
	// 1) Base_Psf: Base Psfs, which represents the main features of each layer(in depth) of Psfs. 
	//	  2-dimension: Row -> depthOfPsf, Column -> numberOfComponent
	// 2) CC2: variablesm, which represents concerning set variables of each Psf.
	//	  2-dimension: Row -> numberOfComponent, Column -> number of Psf(s)
	
	//save Base_Psf and CC2 to files
    std::string PCAname = outName_;
    std::string PCAsuffix;
    std::string PCAprefix;
    size_t PCAi = PCAname.rfind('.', PCAname.length());
    if ( PCAi != std::string::npos )
    {
        PCAprefix = PCAname.substr(0,PCAi);
        PCAsuffix = PCAname.substr(PCAi, PCAname.length());
    }else
	{
		PCAprefix = PCAname;
		PCAsuffix = "";
	}

	// save Base_Psf
	for (int indComponent=0; indComponent< numberOfComponent; indComponent++)
	{
	    std::ostringstream basePsf;
        basePsf << PCAprefix << indComponent << PCAsuffix;
        std::cout <<basePsf.str()<<std::endl;

        // Transform from C++ to matlab format
        for (int i = 0; i < depthOfPsf; i ++)
            Base_PSF_matlab(i, Range::all(), Range::all()) = Base_PSF(Range::all(), Range::all(), i, indComponent);

		wuDataWrite(Base_PSF_matlab, basePsf.str());
	}

	// save CC2 variables
    std::ostringstream psfVariable;
    psfVariable << PCAprefix << "_PCA_Coefficient" << PCAsuffix;
    std::cout <<psfVariable.str()<<std::endl;

	wuDataWrite(CC2, psfVariable.str());

	//Compute and output mean of total
	Array<T, 3> Total_Psf_mean(Size_PSF, Size_PSF, depthOfPsf);
    Array<T, 3> Total_Psf_mean_matlab(depthOfPsf, Size_PSF, Size_PSF);

	//Compute mean
	firstIndex firstI;
	secondIndex secondI;
	thirdIndex thirdI;
	fourthIndex fourthI;

	//Total_Psf_mean = mean(psfs_4d(firstI, secondI, thirdI, fourthI), fourthI);                                      // Ver. 0.9.18
       Total_Psf_mean = (mean(psfs_4d1(firstI, secondI, thirdI, fourthI), fourthI) + mean(psfs_4d2(firstI, secondI, thirdI, fourthI), fourthI))/2; // Ver. 0.9.18

    // Transform matlab format to C++ format
    for (int i = 0; i < depthOfPsf; i ++)
        Total_Psf_mean_matlab(i, Range::all(), Range::all()) = Total_Psf_mean(Range::all(), Range::all(), i);


    std::ostringstream psfBaseZero;
    psfBaseZero << PCAprefix << "_PCA_BaseZero" << PCAsuffix;
    std::cout <<psfBaseZero.str()<<std::endl;

	wuDataWrite(Total_Psf_mean_matlab, psfBaseZero.str());

    return true;

}

template<typename T, int N>
Array<T,N> ToolMgr::IMRESIZE(
    Array<T,N> maskFile,
    int CPMSize
){
    // 1. Rewirting 'imresize' function from Matlab 2009
    // Here we simplifid 'imresize' function for our 2D image

    //global varilables
    int DIM_NUMBER = 2;
    double kernel_width = 4.0;

    // read the object image
    TinyVector<int,N> extent(maskFile.extent());

    // Switch dimensions between 1st and 3rd ones because of different nature of Matlab and C++ languages
    int input_size[] = {extent(0), extent(1)};
    int output_size[] = {CPMSize, CPMSize};

    double scale[] = {(double)output_size[0]/(double)input_size[0], (double)output_size[1]/(double)input_size[1]};
    int order[] = {0, 1};

    //Calculate weights and indices
    Array<Array<double,2>, 1> weights(2);
    Array<Array<int,2>, 1> indices(2);

    for (int k = 0; k < DIM_NUMBER; k++){

        //Function contributions
        bool h = true; //Choose default kernel
        kernel_width = 4.0;  //default value of kernel size

        if (scale[k] < 1){
            h = false;
            kernel_width = kernel_width / scale[k];
        }

        int out_length = output_size[k];

        Array<int, 1> x(out_length);
        for (int i = 0; i< x.extent(0); i++)
        {
            x(i) = i+1;
        }

        Array<double, 1> u(out_length);
        u = x/scale[k] + 0.5*(1 - 1/scale[k]);

        Array<double, 1> left (out_length);
        left = floor(u - kernel_width/2);

        int P = ceil(kernel_width) + 2;

        Array<int,2> indices_tmp(out_length, P);
        for (int i = 0; i < P; i++ ){
            indices_tmp(Range::all(),i) = left + i;
        }

        // Cubic function
        Array<double,2> x_sub(indices_tmp.extent());
        for (int i = 0; i < P; i++ ){
            x_sub(Range::all(),i) = u - indices_tmp(Range::all(),i);
        } 

        if (h == false)
            x_sub = x_sub*scale[k];

        Array<double,2> absx(x_sub.extent()), absx2(x_sub.extent()), absx3(x_sub.extent());
        absx = abs(x_sub);
        absx2 = pow2(absx);
        absx3 = pow3(absx);

        Array<double,2> weights_tmp(x_sub.extent());
        //Blitz+'s dot function doesn't works well?! So we have to use loop for setting value to each item of array
        for (int i = 0; i< x_sub.extent(1); i++)
            for (int j = 0; j < x_sub.extent(0); j++){
                if (absx(j,i) <= 1){
                    weights_tmp(j, i) = 1.5*absx3(j,i) - 2.5*absx2(j,i) + 1;
                }else if ((absx(j,i) > 1) && (absx(j,i) <= 2)){
                    weights_tmp(j, i) = -0.5*absx3(j,i) + 2.5*absx2(j,i) - 4*absx(j,i) + 2;
                } else{ // absx(j,i)>2
                    weights_tmp(j, i) = 0;
                }
            }

        if (h == false)
            weights_tmp = weights_tmp*scale[k];

        //Sum of second dimension
        Array<double, 1> z(weights_tmp.extent(0));
        z = 0;
        firstIndex firstI;
        secondIndex secondI;
        z = sum (weights_tmp(firstI, secondI), secondI);

        for (int i = 0; i < weights_tmp.extent(1); i++ ){
            weights_tmp(Range::all(),i) = weights_tmp(Range::all(),i)/z;
        }

        // If a column in weights is all zero, get rid of it.
        Array<double, 1> zero_flag(weights_tmp.extent(1));
        zero_flag = all(weights_tmp(secondI, firstI) == 0, secondI);

        weights(k).resize(weights_tmp.extent(0), weights_tmp.extent(1) - sum(zero_flag));
        int weight_col = 0;

        for (int i = 0; i < weights_tmp.extent(1); i++){
            if (zero_flag(i) == 0){
                weights(k)(Range::all(), weight_col++) = weights_tmp(Range::all(), i);
            }
        }

        for (int i = 0; i< indices_tmp.extent(1); i++)
            for (int j = 0; j < indices_tmp.extent(0); j++){
                // Translating from matlab code of 'indices = min(max(1, indices), size(A, k))'
                // Here we assume input_size[k] >= 1
                if (indices_tmp(j,i)< 1){
                    indices_tmp(j,i) = 1;
                } else if (indices_tmp(j,i) > input_size[k]){
                    indices_tmp(j,i) = input_size[k];
                }
            }

        indices(k).resize(indices_tmp.extent(0), indices_tmp.extent(1) - sum(zero_flag));
        int indice_col = 0;

        for (int i = 0; i < indices_tmp.extent(1); i++){
            if (zero_flag(i) == 0){
                indices(k)(Range::all(), indice_col++) = indices_tmp(Range::all(), i);
            }
        }

    }// End of Loop k

    // Perform resize process
    Array<T, N> img_B(maskFile.extent(0), maskFile.extent(1));
    for (int i=0; i< img_B.extent(1); i++)
        for (int j=0; j< img_B.extent(0); j++){
            img_B(j,i) = maskFile(j,i);
        }

    for (int k2 = 0; k2 < DIM_NUMBER; k2++){
        int dim = order[k2];

        // Translating 'resizeAlongDim' of Matlab function
        // We prefer to resue kind of 3D algorithm coming from Matlab code
        int out_length = weights(dim).extent(0);

        TinyVector<int,2> size_in(img_B.extent());

        // Translate from Matlabe code 'pseudo_size_in = [prod(size_in(1:dim-1)) size_in(dim) prod(size_in(dim+1:end))];'
        int cmp, fir, sec, thr;
        
        cmp = (dim - 1) - 0;
        if (cmp < 0)
            fir = 1;
        else
            fir = size_in(dim - 1);

        sec = size_in(dim);

        cmp = 1 - (dim + 1);
        if (cmp < 0)
            thr = 1;
        else
            thr = size_in(dim + 1);

        TinyVector<int,3> pseudo_size_in(fir, sec, thr);

        //Reshape img_B to _in based on the shape of pseudo_size_in
        int img_B_fir = 0, img_B_sec = 0;
        Array<T, 3>_in(pseudo_size_in);
        for (int i = 0; i < pseudo_size_in(2); i++)
            for (int j = 0; j < pseudo_size_in(1); j++)
                for (int m = 0; m < pseudo_size_in(0); m++){
                    if ((img_B_fir >= img_B.extent(0)) ||(img_B_sec >= img_B.extent(1))){
                        cout<<"Error: img_B's I/O does over memory!"<<endl;
                        img_B.resize(0, 0);
                        return img_B;
                    }
                    _in(m,j,i) = img_B(img_B_fir, img_B_sec);
                    img_B_fir++;
                    if (img_B_fir >= img_B.extent(0)){
                        img_B_fir = 0;
                        img_B_sec++;
                    }
                }

        int M_ind = pseudo_size_in(0);
        int P_ind = pseudo_size_in(2);

        TinyVector<int,3> pseudo_size_out(M_ind, out_length, P_ind);

        Array<T, 3> _out(pseudo_size_out);

        _out = 0;

        int block_size = 15;

        for (int m = 1; m <= M_ind; ){
            for (int p = 1; p <= P_ind; ){
                Array<int, 1> mm, pp;
                int tail_ind, cur_ind;

                if (m + block_size - 1 < M_ind){
                    tail_ind = m + block_size - 1;
                } else{
                    tail_ind = M_ind;
                }

                cur_ind = 0;
                mm.resize(tail_ind - m + 1);
                for (int i = m; i<= tail_ind; i++){
                    mm(cur_ind++) = i;
                }

                if (p + block_size - 1 < P_ind){
                    tail_ind = p + block_size - 1;
                } else{
                    tail_ind = P_ind;
                }

                cur_ind = 0;
                pp.resize(tail_ind - p + 1);
                for (int i = p; i<= tail_ind; i++){
                    pp(cur_ind++) = i;
                }

                Array<T, 3> block (mm.extent(0), _in.extent(1), pp.extent(0));
                    
                block(Range::all(), Range::all(), Range::all()) = _in(Range(mm(0)-1, mm(mm.extent(0)-1)-1), 
                    Range::all(), Range(pp(0)-1, pp(pp.extent(0)-1)-1));

                //permute(block, [2 1 3])
                Array<T, 3>block_tmp(block.extent(1), block.extent(0), block.extent(2));
                for (int i = 0; i < block_tmp.extent(2); i++)
                    for (int j = 0; j < block_tmp.extent(1); j++)
                        for (int m = 0; m < block_tmp.extent(0); m++){
                            block_tmp(m,j,i) = block(j,m,i);
                        }

                //block = resizeColumns(block, weights, indices); BY block_tmp
                //Matlab 2009 resizeColumnsCore
                //Preprocess for input agruments
                //Translate Matlab data to C kind data structure
                
                //  pr_in = Block_tmp
                T *pr_in =(T *) malloc(block_tmp.extent(0)*block_tmp.extent(1)*block_tmp.extent(2)*sizeof(T));
                int pr_in_col = 0;
                for (int i = 0; i < block_tmp.extent(2); i++)
                    for (int j = 0; j < block_tmp.extent(1); j++)
                        for (int m = 0; m < block_tmp.extent(0); m++)
                            pr_in[pr_in_col++] = block_tmp(m,j,i);

                //  weights_mex = weights(dim)'
                double *weights_mex =(double *) malloc(weights(dim).extent(1)*weights(dim).extent(0)*sizeof(double));
                int weights_mex_col = 0;
                for (int i = 0; i < weights(dim).extent(0); i++)
                    for (int j = 0; j < weights(dim).extent(1); j++)
                    {
                        weights_mex[weights_mex_col++] = weights(dim)(i,j);
                    }


                //  indices_mex = indices(dim)'
                //  And precompute the zero-based indices
                int *indices_mex =(int *) malloc(indices(dim).extent(1)*indices(dim).extent(0)*sizeof(int));
                int indices_mex_col = 0;
                for (int i = 0; i < indices(dim).extent(0); i++)
                    for (int j = 0; j < indices(dim).extent(1); j++)
                    {
                        indices_mex[indices_mex_col++] = indices(dim)(i,j) - 1;
                    }

                //Entity of function
                int M_in = block_tmp.extent(0);
                int core_N = block_tmp.extent(1);
                int core_P = block_tmp.extent(2);

                int M_out = weights(dim).extent(0);
                int num_weights_per_pixel = weights(dim).extent(1);

                //  Output variable
                T *core_out =(T *) malloc(M_out*core_N*core_P*sizeof(T));

                //  ComputeWeightedSums() function
                int num_cols = core_N* core_P;

                for (int k = 0; k < num_cols; k++){
                    T *v_in = &pr_in[M_in * k];
                    T *v_out = &core_out[M_out * k];
                    double *w = weights_mex;
                    int *idx = indices_mex;

                    for (int m = 0; m < M_out; m++){
                        T sum =0.0;
                        for (int p = 0; p < num_weights_per_pixel; p++){
                            if (k == 0 ){
                            }
                            sum += *w++ * v_in[*idx++];
                        }
                        *v_out++ = sum;
                    }
                }
                //End of ComputeWeightedSums() function

                //Post process 
                //Translate C kind output datastructure to Matlab kind (core_out -> block_tmp)
                // 1) resize 
                block_tmp.resize(M_out, core_N, core_P);

                // 2) Assign values
                int core_out_col = 0;
                for (int i = 0; i < block_tmp.extent(2); i++)
                    for (int j = 0; j < block_tmp.extent(1); j++)
                        for (int m = 0; m < block_tmp.extent(0); m++)
                        block_tmp(m,j,i) = core_out[core_out_col++];

                //Free memory
                free(pr_in);
                free(weights_mex);
                free(indices_mex);
                free(core_out);

                //End of resizeColumnsCore() function

                //out(mm, :, pp) = ipermute(block, [2 1 3]);
                //1) block = ipermute(block_tmp, [2 1 3])
                block.resize(block_tmp.extent(1), block_tmp.extent(0), block_tmp.extent(2));
                for (int i = 0; i < block.extent(2); i++)
                    for (int j = 0; j < block.extent(1); j++)
                        for (int m = 0; m < block.extent(0); m++){
                            block(m,j,i) = block_tmp(j,m,i);
                        }

                //2)out(mm, :, pp) = block(:,:,:);
                for (int i = 0; i < block.extent(2); i++)
                    for (int j = 0; j < block.extent(1); j++)
                        for (int m = 0; m < block.extent(0); m++){
                            _out(m+mm(0)-1,j,i+pp(0)-1) = block(m,j,i);
                        }

                //For loop of p
                p = p + block_size;
            }
            //For loop of m
            m = m + block_size;
        }

        TinyVector<int,2> size_out;
        size_out(1 - dim) = size_in(1 - dim);
        size_out(dim) = out_length;

        //Reshape out to img_B based on the shape of size_out
        img_B.resize(size_out);
        img_B = 0;
        int out_fir = 0, out_sec = 0, out_thi = 0;
        for (int i = 0; i < img_B.extent(1); i++)
            for (int j = 0; j < img_B.extent(0); j++){
                if ((out_fir >= _out.extent(0)) ||(out_sec >= _out.extent(1))||(out_thi >= _out.extent(2))){
                    cout<<"Error: out's I/O does over memory!"<<endl;
                    img_B.resize(0,0);;
                    return img_B;
                }
                img_B(j, i) = _out(out_fir, out_sec, out_thi);
                out_fir++;
                if (out_fir >= _out.extent(0)){
                    out_fir = 0;
                    out_sec++;
                    if (out_sec >= _out.extent(1)){
                        out_sec = 0;
                        out_thi++;
                    }
                }
            }
    }// End of Loop k2

    cout <<"Mask file's size has been changed from ["<<extent(0)<<"*"<<extent(1)<<"] to ["<<CPMSize<<"*"<<CPMSize<<"]"<<endl;
    
    return img_B;
}


template<typename T, int N>
Array<T,N> ToolMgr::CPMWithFFT(
	bool centeredPsf
){

    //forward FFT_SIGN = -1, backword FFT_SIGN = 1
    int FFT_SIGN = -1;

    //1. read the PSF file, including real and imaginary parts
    Array<T, N> psf_real;
    Array<T, N> psf_imag;

    std::string psfname = inName_;
    std::string psfsuffix;
    std::string psfprefix;
    size_t i = psfname.rfind('.', psfname.length());
    if ( i != std::string::npos )
    {
        psfprefix = psfname.substr(0,i);
        psfsuffix = psfname.substr(i, psfname.length());
    }

    std::ostringstream str_psf_real;
    std::ostringstream str_psf_imag;

    str_psf_real << psfprefix << "_re" << psfsuffix;
    str_psf_imag << psfprefix << "_im" << psfsuffix;
    std::cout <<str_psf_real.str()<<std::endl;
    std::cout <<str_psf_imag.str()<<std::endl;
    wuDataRead(psf_real, str_psf_real.str());
    wuDataRead(psf_imag, str_psf_imag.str());

    // Read Psf's header information for resize Mask file
    wuHeader psfHeader;
    psfHeader.read(psfname);
    int nx = psfHeader.columns();
    float na = psfHeader.aperture();
    int wave = psfHeader.wavelength();
    float spacing = psfHeader.spacing();

    // Formula: CPM# = 2* NA/WAVE * N * SPACING
    int CPM_SIZE = floor(2*(na*1000/wave)*nx*spacing);
    std::cout <<"Numerical Aperture: "<<na<<", wave length: "<<wave <<", X spacing: "<<spacing <<", and X deminsion: "<< nx << std::endl;
    std::cout <<"CPM # is: "<<CPM_SIZE<<std::endl;

    //Combine real and imaginary parts of PSF
    Array<std::complex<T>, N> psf_complex(psf_real.extent());
    
    real(psf_complex) = psf_real;
    imag(psf_complex) = psf_imag;

    //2. read the mask file
    Array<T, N-1> mask_real;
    Array<T, N> mask_real_tmp;
    std::cout <<refName_<<std::endl;
    wuDataRead(mask_real_tmp, refName_);

    int ind_flag = 0;

    if (mask_real_tmp.extent(0)<=1){
        mask_real.resize(mask_real_tmp.extent(2), mask_real_tmp.extent(1));
        for (int i=0; i< mask_real.extent(1); i++)
            for (int j=0; j< mask_real.extent(0); j++){
                mask_real(j,i) = mask_real_tmp(0,i,j);
            }
        ind_flag++;
    }

    if (mask_real_tmp.extent(1)<=1) {
        mask_real.resize(mask_real_tmp.extent(0), mask_real_tmp.extent(2));
        for (int i=0; i< mask_real.extent(1); i++)
                for (int j=0; j< mask_real.extent(0); j++){
                    mask_real(j,i) = mask_real_tmp(j,0,i);
                }
        ind_flag++;
    }

    if (mask_real_tmp.extent(2)<=1) {
        mask_real.resize(mask_real_tmp.extent(0), mask_real_tmp.extent(1));
        for (int i=0; i< mask_real.extent(1); i++)
                for (int j=0; j< mask_real.extent(0); j++){
                    mask_real(j,i) = mask_real_tmp(j,i,0);
                }
        ind_flag++;
    }

    if ((ind_flag == 0) || (ind_flag > 1)){
        cout <<"Error: Input mask file is not a valid 2D file"<<endl;
        psf_real.resize(0,0,0);
        return psf_real;
    }

    // Resize mask file, by rewirting 'imresize' Matlab code
    Array<T, N-1> new_mask_real = IMRESIZE<T, N-1>(mask_real, CPM_SIZE);

    if (new_mask_real.size() == 0){
        psf_real.resize(0,0,0);
        return psf_real;
    }

    Array<std::complex<T>, N-1> mask_complex(new_mask_real.extent());

    real(mask_complex) = cos(new_mask_real);
    imag(mask_complex) = sin(new_mask_real);

    //Do convolution between mask and each plane of PSF
    //2D Psf of each plane
    Array<std::complex<T>, N-1> psf_plane(psf_complex.extent(1), psf_complex.extent(2));

    Array<std::complex<T>, N-1> psfF(psf_plane.extent());
    Array<std::complex<T>, N-1> maskResizedF(psf_plane.extent());
    Array<std::complex<T>, N-1> newPsfF(psf_plane.extent());
    Array<std::complex<T>, N-1> psf_plane_new(psf_plane.extent());
    fftwInterface<T,N-1> fftw;
    Array<T, N> psf_simple(psf_real.extent());
    Array<T, N> psf_nor(psf_real.extent());
  
    //Along z dimension of PSF
    for (int m = 0; m < psf_complex.extent(0); m++ )
    {
        //Get each PSF layer
        psf_plane(Range::all(), Range::all()) = psf_complex(m, Range::all(), Range::all());

        if (centeredPsf == true){
            //Center -> Uncenter
            TinyVector<int,3-1> shift1((psf_plane.shape()+1)/2);
            psf_plane = cosm::circularShiftC2C(psf_plane, shift1);
        }

        //FFTW function
        psfF = cosm::forwardFFTC2C(psf_plane);

        //Uncenter -> Center
        TinyVector<int,3-1> shift2((psfF.shape()+1)/2);
        psfF = cosm::circularShiftC2C(psfF, shift2);

        //Mask Resize
        cosm::padAround(mask_complex, maskResizedF);

        //Mask file is aready on Fourier domain, so just mutiply between it and PSF
        newPsfF = psfF * maskResizedF;

        //Center -> Uncenter
        TinyVector<int,3-1> shift3((newPsfF.shape()+1)/2);
        newPsfF = cosm::circularShiftC2C(newPsfF, shift3);

        // convert back to time domain (Complex to Complex)
        fftw.plan(newPsfF, psf_plane_new, FFT_SIGN);
        fftw.execute();
        psf_plane_new /= psf_plane_new.size();

        if (centeredPsf == true){
            //Uncenter -> Center
            TinyVector<int,3-1> shift((psf_plane_new.shape()+1)/2);
            psf_plane_new = cosm::circularShiftC2C(psf_plane_new, shift);
        }

        psf_simple(m, Range::all(), Range::all()) = sqr(real(psf_plane_new)) + sqr(imag(psf_plane_new));

        cout << "depth index: " << m+1 << " / " << psf_complex.extent(0) << endl;
    }

    psf_nor = psf_simple/sum(psf_simple);

    return psf_nor;

}

};

#endif // TOOL_MGR_H
