#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <windows.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <complex.h>
int nextpow2(int);
double sinc(double);


#pragma pack(push, 1)
typedef struct {
	uint16_t type;
	uint32_t size;
	uint16_t reserved1;
	uint16_t reserved2;
	uint32_t offset;
	uint32_t dib_header_size;
	int32_t width_px;
	int32_t height_px;
	uint16_t num_planes;
	uint16_t bits_per_pixel;
	uint32_t compression;
	uint32_t image_size_bytes;
	int32_t x_resolution_ppm;
	int32_t y_resolution_ppm;
	uint32_t num_colors;
	uint32_t important_colors;
} BMPHeader;
#pragma pack(pop)

void write_bmp_header(FILE* file, int width, int height) {
	BMPHeader header = { 0 };
	int row_padded = (width * 3 + 3) & (~3);
	int data_size = row_padded * height;
	int file_size = 54 + data_size;

	header.type = 0x4D42;
	header.size = file_size;
	header.offset = 54;
	header.dib_header_size = 40;
	header.width_px = width;
	header.height_px = height;
	header.num_planes = 1;
	header.bits_per_pixel = 24;
	header.image_size_bytes = data_size;

	fwrite(&header, sizeof(BMPHeader), 1, file);
}

void save_matrix_as_bmp(double** matrix, int width, int height, const char* filename) {
	FILE* file = fopen(filename, "wb");
	if (!file) {
		fprintf(stderr, "Impossible d'ouvrir le fichier %s\n", filename);
		return;
	}

	write_bmp_header(file, width, height);

	int row_padded = (width * 3 + 3) & (~3);
	uint8_t padding[3] = { 0, 0, 0 };
	int padding_size = row_padded - (width * 3);

	// Trouver les valeurs min et max dans la matrice pour la normalisation
	double min_val = matrix[0][0];
	double max_val = matrix[0][0];
	for (int y = 0; y < height; y++) {
		for (int x = 0; x < width; x++) {
			if (matrix[y][x] < min_val) min_val = matrix[y][x];
			if (matrix[y][x] > max_val) max_val = matrix[y][x];
		}
	}

	for (int y = height - 1; y >= 0; y--) {
		for (int x = 0; x < width; x++) {
			// Normaliser et convertir en uint8_t
			uint8_t pixel = (uint8_t)(255 * (matrix[y][x] - min_val) / (max_val - min_val));
			uint8_t color[3] = { pixel, pixel, pixel };
			fwrite(color, 3, 1, file);
		}
		fwrite(padding, padding_size, 1, file);
	}

	fclose(file);
}




#define pi 3.1416
#define eps 2.2204e-16
#define _CRT_SECURE_NO_WARNINGS 
#define DATA_DIR L"C:\\Users\\yazan\\Desktop\\07_07_2024\\Stage ijl\\UTSR_CPP\\EssayScriptExample\\AcquiredData\\Specimen01\\*"
#define MAX_FILENAME_LEN 256
#define MAX_DATA_LEN 10000


void main()
{
	printf("**************** run main ********************* \n");
	// Defines essay parameters
	// Propagation medium and transducer parameters
	double TendGate, cl, d, fc, bw, A0, alpha_atenu, T1;
	TendGate = 24.98; cl = 5859.4; d = 6.35;
	fc = 4.6; bw = 4.4; A0 = 2.02;
	alpha_atenu = 0.03548; T1 = 100e-9;
	// Generating a infinitesimal point discontinuity model for calibration
	// ROI(in mm)
	double minX, maxX, minZ, maxZ, DepthCentre;
	minX = 0; maxX = 30;
	minZ = 40; maxZ = 41.7;
	DepthCentre = 40;
	//simulNDT
	double deltaT_simul;
	int TsGate_simul, AscanPoints_simul, Rows_simul, Diameter_simul;
	deltaT_simul = 0.02;
	TsGate_simul = 0;
	AscanPoints_simul = (int)(TendGate / deltaT_simul) + 1;
	Rows_simul = (int)(maxX)+1;
	Diameter_simul = 0;
	double** timeScale_simul = (double**)malloc(AscanPoints_simul * sizeof(double*));
	int** X_simul = (int**)malloc(Rows_simul * sizeof(int*));
	for (int i = 0; i < AscanPoints_simul; i++)
	{
		timeScale_simul[i] = (double*)malloc(1 * sizeof(double));
		timeScale_simul[i][0] = i * deltaT_simul;
	}
	for (int i = 0; i < Rows_simul; i++)
	{
		X_simul[i] = (int*)malloc(1 * sizeof(int));
		X_simul[i][0] = i;
	}
	// Getting calibration data
	double ERMv, du;
	ERMv = cl / 2;
	du = (X_simul[1][0] - X_simul[0][0]) * 1e-3;
	// GenerateModelFilter
	double deltaScanX_g, deltaT_g, y_g;
	int Rx_g, nt0_g, nu0_g, nt_g, nu_g;
	double** scanX_g = (double**)malloc(Rows_simul * sizeof(double*));

	Rx_g = 1;
	for (int i = 0; i < Rows_simul; i++)
	{
		scanX_g[i] = (double*)malloc(1 * sizeof(double));
		scanX_g[i][0] = X_simul[i][0] * 1e-3;
	}
	deltaScanX_g = (scanX_g[1][0] - scanX_g[0][0]) / Rx_g;
	deltaT_g = (timeScale_simul[1][0] - timeScale_simul[0][0]) * 1e-6;
	nt0_g = AscanPoints_simul;
	nu0_g = Rows_simul;
	nu0_g = (nu0_g - 1) * Rx_g + 1;
	nt_g = pow(2, nextpow2(nt0_g) + 1);
	nu_g = 2 * nu0_g;

	double** f_g = (double**)malloc(((double)nt_g) * sizeof(double*));
	double** kx_g = (double**)malloc(((double)nu_g) * sizeof(double*));

	for (int i = 0; i < nt_g; i++)
	{
		f_g[i] = (double*)malloc(1 * sizeof(double));
		f_g[i][0] = (((-nt_g / 2) + i) / deltaT_g) / nt_g;
	}
	for (int i = 0; i < nu_g; i++)
	{
		kx_g[i] = (double*)malloc(1 * sizeof(double));
		kx_g[i][0] = (((-nu_g / 2) + i) / deltaScanX_g) / nu_g;
	}
	// meshgrid(kx_g, f_g)
	double** gf = (double**)malloc(((double)nt_g) * sizeof(double*));
	double** gkx = (double**)malloc(((double)nt_g) * sizeof(double*));
	for (int i = 0; i < nt_g; i++)
	{
		gf[i] = (double*)malloc(((double)nu_g) * sizeof(double));
		gkx[i] = (double*)malloc(((double)nu_g) * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			gf[i][j] = f_g[i][0];
			gkx[i][j] = kx_g[j][0];
		}
	}// fin meshgrid(kx_g, f_g)
	double gst;
	int** mask_g = (int**)malloc(nt_g * sizeof(int*));

	for (int i = 0; i < nt_g; i++)
	{
		mask_g[i] = (int*)malloc(nu_g * sizeof(int*));
		for (int j = 0; j < nu_g; j++)
		{
			gst = gkx[i][j] / (gf[i][j] / ERMv);

			if (fabs(gst) <= 1.0)
			{
				mask_g[i][j] = 1;
			}
			else
			{
				mask_g[i][j] = 0;
			}
		}
	}
	//jinc
	double** C1 = (double**)malloc(((double)nt_g) * sizeof(double*));
	//C1
	for (int i = 0; i < nt_g; i++)
	{
		C1[i] = (double*)malloc(((double)nu_g) * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			y_g = (_j1(((pi * gkx[i][j]) * ((d * 1e-3) / 2))) / (((pi * gkx[i][j]) * ((d * 1e-3) / 2))));
			if (isnan(y_g))
			{
				y_g = 0.5;
			}
			C1[i][j] = ((y_g * y_g) * mask_g[i][j]) * ((pi / 2) * ((2 * (d * 1e-3 / 2) * (d * 1e-3 / 2) * (2 * pi) / ERMv) * fabs(f_g[i][0])));
		}
	}// fin jinc

	// cossquare
	double** Hed_r = (double**)malloc(((double)nt_g) * sizeof(double*));
	double** Hed_i = (double**)malloc(((double)nt_g) * sizeof(double*));
	double f1_g, f4_g, ss_g, ff1_g, ff2_g, ri1_g, ri2_g, r1_g, r2_g, r_g, ExcTransd_r, ExcTransd_i;
	f1_g = (fc * 1e6) - (bw * 1e6);
	f4_g = (fc * 1e6) + (bw * 1e6);
	//Hed
	for (int i = 0; i < nt_g; i++)
	{
		Hed_r[i] = (double*)malloc(1 * sizeof(double));
		Hed_i[i] = (double*)malloc(1 * sizeof(double));
		ss_g = sin(pi * f_g[i][0] / (2 * (fc * 1e6)));
		ri1_g = (fabs(f_g[i][0]) > (fc * 1e6)) && (fabs(f_g[i][0]) <= f4_g);
		ri2_g = (fabs(f_g[i][0]) <= (fc * 1e6)) && (fabs(f_g[i][0]) <= f4_g);
		ff1_g = ri1_g * fabs(f_g[i][0]);
		ff2_g = ri2_g * fabs(f_g[i][0]);
		r1_g = ri1_g * (cos(pi * ((fc * 1e6) - ff1_g) / (f4_g - f1_g)) * cos(pi * ((fc * 1e6) - ff1_g) / (f4_g - f1_g)));
		r2_g = ri2_g * ss_g * (cos(pi * ((fc * 1e6) - ff2_g) / (f4_g - f1_g)) * cos(pi * ((fc * 1e6) - ff2_g) / (f4_g - f1_g)));
		if (f_g[i][0] == 0)
		{
			r_g = r1_g;
		}
		else
		{
			r_g = r1_g + copysign(1.0, f_g[i][0] == 0) * r2_g;
		}
		ExcTransd_r = 2 * pi * sinc((2 * pi * f_g[i][0] * (T1 / 2))) * (-cos(-2 * pi * f_g[i][0] * (T1 / 2)));
		ExcTransd_i = 2 * pi * sinc((2 * pi * f_g[i][0] * (T1 / 2))) * (-sin(-2 * pi * f_g[i][0] * (T1 / 2)));

		if (fabs(f_g[i][0]) <= 10e6)
		{
			Hed_r[i][0] = 1.5 * pi * r_g * r_g * ExcTransd_r;
			Hed_i[i][0] = 1.5 * pi * r_g * r_g * ExcTransd_i;
		}
		else
		{
			Hed_r[i][0] = 0.0;
			Hed_i[i][0] = 0.0;
		}
	}//

	double** Model_r = (double**)malloc(((double)nt_g) * sizeof(double*));
	double** Model_i = (double**)malloc(((double)nt_g) * sizeof(double*));
	double** Filt_r = (double**)malloc(((double)nt_g) * sizeof(double*));
	double** Filt_i = (double**)malloc(((double)nt_g) * sizeof(double*));
	for (int i = 0; i < nt_g; i++)
	{
		Model_r[i] = (double*)malloc(((double)nu_g) * sizeof(double));
		Model_i[i] = (double*)malloc(((double)nu_g) * sizeof(double));
		Filt_r[i] = (double*)malloc(((double)nu_g) * sizeof(double));
		Filt_i[i] = (double*)malloc(((double)nu_g) * sizeof(double));

		for (int j = 0; j < nu_g; j++)
		{
			Model_r[i][j] = C1[i][j] * Hed_r[i][0];
			Model_i[i][j] = C1[i][j] * Hed_i[i][0];
			//conj(model)
			Filt_r[i][j] = C1[i][j] * Hed_r[i][0];
			Filt_i[i][j] = -C1[i][j] * Hed_i[i][0];
		}
	}
	/* save_matrix_as_bmp(Model_i, nu_g, nt_g, "module.bmp");
	 const char* filename1 = "module.bmp";
	 #ifdef _WIN32
		 system("start module.bmp");
	 #elif __APPLE__
		 system("open module.bmp");
	 #elif __linux__
		 system("xdg-open module.bmp");
	 #else
		 printf("Image sauvegardée sous %s\n", filename1);
	 #endif*/

	 // fin GenerateModelFilter
	free(timeScale_simul); free(X_simul);
	free(scanX_g); free(f_g);
	free(kx_g); free(gf);
	free(gkx); free(mask_g); free(C1);
	free(Hed_r); free(Hed_i);
	//Defines regularization parameter
	double alpha = 0.02;
	// Loading acquired data
	FILE* file = fopen("AscanValuesL.txt", "r");
	if (file == NULL) {
		perror("Erreur lors de l'ouverture du fichier");
		return 1;
	}
	int rows = 0;
	int cols = 0;
	int temp;
	char line[1024];
	while (fgets(line, sizeof(line), file)) {
		cols = 0;
		char* ptr = line;
		while (sscanf_s(ptr, "%d", &temp) == 1) {
			cols++;
			while (*ptr != '\0' && *ptr != ' ' && *ptr != '\t') ptr++;
			while (*ptr == ' ' || *ptr == '\t') ptr++;
		}
		rows++;
	}
	// printf("rows=%d,cols=%d\n", rows, cols);
	double** AscanValues_ptAco40dB = (double**)malloc(((double)rows) * sizeof(double*));
	for (int i = 0; i < rows; i++) {
		AscanValues_ptAco40dB[i] = (double*)malloc(((double)cols) * sizeof(double));
	}
	rewind(file);
	int i = 0, j = 0;
	while (fgets(line, sizeof(line), file)) {
		j = 0;
		char* ptr = line;
		while (sscanf_s(ptr, "%d", &temp) == 1) {
			AscanValues_ptAco40dB[i][j++] = (double)temp * (double)((0.5 / 2048) / 100);
			while (*ptr != '\0' && *ptr != ' ' && *ptr != '\t') ptr++;
			while (*ptr == ' ' || *ptr == '\t') ptr++;
		}
		i++;
	}
	fclose(file);

	double deltaT_Load, sum_Load;
	int backDepth_Load, Frequency_ptAco40dB;
	double Cl_ptAco40dB, ProbeDiameter_ptAco40dB, Wavelength_ptAco40dB;
	int backWallIdx[1][2] = { 1029,1029 };
	deltaT_Load = 0.02;
	backDepth_Load = 60;

	double** timeScale_ptAco40dB = (double**)malloc((rows) * sizeof(double*));

	for (int i = 0; i < rows; i++)
	{
		timeScale_ptAco40dB[i] = (double*)malloc(1 * sizeof(double));
		timeScale_ptAco40dB[i][0] = deltaT_Load * i;
	}
	double** X_ptAco40dB = (double**)malloc(((double)cols) * sizeof(double*));
	for (int i = 0; i < cols; i++)
	{
		X_ptAco40dB[i] = (double*)malloc(1 * sizeof(double));
		X_ptAco40dB[i][0] = i;
	}
	sum_Load = 0;
	for (int i = 0; i < 2; i++)
	{
		int var_x = backWallIdx[0][i] - 1;
		sum_Load = sum_Load + ((backDepth_Load * 2000) / (timeScale_ptAco40dB[var_x][0]));
	}
	Cl_ptAco40dB = sum_Load / 2;
	Frequency_ptAco40dB = 5;
	ProbeDiameter_ptAco40dB = 6.35;
	Wavelength_ptAco40dB = Cl_ptAco40dB / (Frequency_ptAco40dB * 1000);
	double** zz_Load = (double**)malloc((rows) * sizeof(double*));
	double** ffc_Load = (double**)malloc((rows) * sizeof(double*));
	double var_x1 = 0;
	for (int i = 0; i < rows; i++)
	{
		zz_Load[i] = (double*)malloc(1 * sizeof(double));
		ffc_Load[i] = (double*)malloc(1 * sizeof(double));
		zz_Load[i][0] = (timeScale_ptAco40dB[i][0] * Cl_ptAco40dB) / 2000;
		var_x1 = (1 / A0) * exp(alpha_atenu * zz_Load[i][0]);
		if (var_x1 < 1)
		{
			ffc_Load[i][0] = 1;
		}
		else
		{
			ffc_Load[i][0] = var_x1;
		}
	}
	for (int i = 0; i < rows; i++)
	{
		for (int j = 0; j < cols; j++)
		{
			AscanValues_ptAco40dB[i][j] = AscanValues_ptAco40dB[i][j] * ffc_Load[i][0];
		}
	}
	//    save_matrix_as_bmp(AscanValues_ptAco40dB, cols, rows, "AscanValues_ptAco40dB.bmp");
	//    const char* filename2 = "AscanValues_ptAco40dB.bmp";
	//#ifdef _WIN32
	//    system("start AscanValues_ptAco40dB.bmp");
	//#elif __APPLE__
	//    system("open AscanValues_ptAco40dB.bmp");
	//#elif __linux__
	//    system("xdg-open AscanValues_ptAco40dB.bmp");
	//#else
	//    printf("Image sauvegardée sous %s\n", filename2);
	//#endif

		/// UTSR_ModS2
	double ERMv1, scanY, deltaScanZ, apertAngle, tanApertAngle, limApertX, tau0, dt, beta, tol, cg_tol;
	int idxMinZ, idxMaxZ, M, colIniX, Rx;
	ERMv1 = Cl_ptAco40dB / 2;
	double** scanX = (double**)malloc(((double)cols) * sizeof(double*));
	for (int i = 0; i < cols; i++)
	{
		scanX[i] = (double*)malloc(1 * sizeof(double));
		scanX[i][0] = X_ptAco40dB[i][0] * 1e-3;
	}
	double** scanZ = (double**)malloc((rows) * sizeof(double*));
	for (int i = 0; i < rows; i++)
	{
		scanZ[i] = (double*)malloc(1 * sizeof(double));
		scanZ[i][0] = timeScale_ptAco40dB[i][0] * 1e-6 * ERMv1;
	}
	deltaScanZ = scanZ[1][0] - scanZ[0][0];
	apertAngle = asin((0.51 * Cl_ptAco40dB) / (Frequency_ptAco40dB * 1e6 * ProbeDiameter_ptAco40dB * 1e-3));
	tanApertAngle = tan(apertAngle);
	for (int i = 0; i < rows; i++)
	{

		if ((scanZ[i][0]) < ((double)(minZ * 1e-3)))
		{
			idxMinZ = i + 1;
		}
		if ((scanZ[i][0]) < ((double)(maxZ * 1e-3)))
		{
			idxMaxZ = i + 2;
		}
	}
	M = (1 + (idxMaxZ - idxMinZ));
	double** roiZ = (double**)malloc((double)M * sizeof(double*));
	for (int i = 0; i < M; i++)
	{
		roiZ[i] = (double*)malloc(1 * sizeof(double));
		roiZ[i][0] = scanZ[i + idxMinZ][0];
	}
	limApertX = roiZ[M - 1][0] * tanApertAngle;
	for (int i = 0; i < cols; i++)
	{
		if ((scanX[i][0] + limApertX) >= scanX[0][0])
		{
			colIniX = i + 1;
			break;
		}
	}
	printf("$$$$$$$ Starting UTSR algorithm (alpha = %f) $$$$$$$\n", alpha);
	printf("---- Title: UTSR solution ----\n");
	clock_t tAlg = clock();
	//----- Starting UTSR algorithm -----
	double** S_t_u = (double**)malloc((double)M * sizeof(double*));
	for (int i = 0; i < M; i++)
	{
		S_t_u[i] = (double*)malloc(cols * sizeof(double));
		for (int j = 0; j < cols; j++)
		{
			S_t_u[i][j] = AscanValues_ptAco40dB[i + idxMinZ - 1][j];
		}
	}
	//    save_matrix_as_bmp(S_t_u, cols, M, "S_t_u.bmp");
	//    const char* filename3 = "S_t_u.bmp";
	//#ifdef _WIN32
	//    system("start S_t_u.bmp");
	//#elif __APPLE__
	//    system("open S_t_u.bmp");
	//#elif __linux__
	//    system("xdg-open S_t_u.bmp");
	//#else
	//    printf("Image sauvegardée sous %s\n", filename3);
	//#endif
	int fOrig = NULL;
	tau0 = timeScale_ptAco40dB[idxMinZ - 1][0] * 1e-6;
	int szData[1][2] = { M,cols };
	dt = deltaScanZ / ERMv1;
	beta = 1;
	tol = 1e-2;
	cg_tol = 1e-2;
	Rx = 1;
	// ModS2T(S_t_u, dt, dx, ERMv, tau0, Filt, Rx)
	//meshgrid(ku,f)
	printf("nt_g=%d, nu_g=%d, M=%d, cols=%d\n", nt_g, nu_g, M, cols);

	double** f_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ku_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** fkz_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double sign_f;
	for (int i = 0; i < nt_g; i++)
	{
		f_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		ku_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		fkz_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			f_ModS2T[i][j] = (((double)(-nt_g / 2.0) + i) / dt) / nt_g;
			ku_ModS2T[i][j] = (((double)(-nu_g / 2.0) + j) / du) / nu_g;
			sign_f = (f_ModS2T[i][j] > 0) - (f_ModS2T[i][j] < 0);
			fkz_ModS2T[i][j] = ERMv1 * sign_f * sqrt((ku_ModS2T[i][j] * ku_ModS2T[i][j]) + ((f_ModS2T[i][j] / ERMv1) * (f_ModS2T[i][j] / ERMv1)));
			// printf("%e\t", f_ModS2T[i][j]);
		}
		// printf("\n");
	}



		// start timing for FFT
	
	// FFT2(S_t_u, nt_g, nu_g)
	double sum_ftimage1_r_ModS2T = 0.0;
	double sum_ftimage1_i_ModS2T = 0.0;
	double** ftimage1_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage1_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	for (int k1 = 0; k1 < nt_g; k1++)
	{
		ftimage1_r_ModS2T[k1] = (double*)malloc(nu_g * sizeof(double));
		ftimage1_i_ModS2T[k1] = (double*)malloc(nu_g * sizeof(double));
		for (int k2 = 0; k2 < nu_g; k2++)
		{
			for (int n1 = 0; n1 < M; n1++)
			{
				for (int n2 = 0; n2 < cols; n2++)
				{
					double angle = 2 * pi * ((double)(k1 * n1) / nt_g + (double)(k2 * n2) / nu_g);
					sum_ftimage1_r_ModS2T = sum_ftimage1_r_ModS2T + (S_t_u[n1][n2] * cos(angle));
					sum_ftimage1_i_ModS2T = sum_ftimage1_i_ModS2T - S_t_u[n1][n2] * sin(angle);
				}
			}
			ftimage1_r_ModS2T[k1][k2] = sum_ftimage1_r_ModS2T;
			ftimage1_i_ModS2T[k1][k2] = sum_ftimage1_i_ModS2T;
			sum_ftimage1_i_ModS2T = 0.0;
			sum_ftimage1_r_ModS2T = 0.0;
		}
	}

	//fftshift
	double** ftimage21_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage21_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));

	int a_fftshift, b_fftshift;
	if (nt_g % 2 == 0)
	{
		a_fftshift = 0;
	}
	else {
		a_fftshift = 1;
	}
	if (nu_g % 2 == 0)
	{
		b_fftshift = 0;
	}
	else
	{
		b_fftshift = 1;
	}
	for (int i = 0; i < nt_g; i++)
	{
		ftimage21_r_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage21_i_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{

			if (b_fftshift == 1)
			{
				if (j < (int)(nu_g / 2))
				{
					ftimage21_r_ModS2T[i][j] = ftimage1_r_ModS2T[i][j + (int)(nu_g / 2) + 1];
					ftimage21_i_ModS2T[i][j] = ftimage1_i_ModS2T[i][j + (int)(nu_g / 2) + 1];
				}
				else
				{
					ftimage21_r_ModS2T[i][j] = ftimage1_r_ModS2T[i][j - (int)(nu_g / 2)];
					ftimage21_i_ModS2T[i][j] = ftimage1_i_ModS2T[i][j - (int)(nu_g / 2)];
				}
			}
			if (b_fftshift == 0)
			{
				if (j < (int)(nu_g / 2))
				{
					ftimage21_r_ModS2T[i][j] = ftimage1_r_ModS2T[i][j + (int)(nu_g / 2)];
					ftimage21_i_ModS2T[i][j] = ftimage1_i_ModS2T[i][j + (int)(nu_g / 2)];
				}
				else
				{
					ftimage21_r_ModS2T[i][j] = ftimage1_r_ModS2T[i][j - (int)(nu_g / 2)];
					ftimage21_i_ModS2T[i][j] = ftimage1_i_ModS2T[i][j - (int)(nu_g / 2)];
				}
			}
		}
	}

	double** ftimage2_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage2_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	for (int i = 0; i < nt_g; i++)
	{
		ftimage2_r_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage2_i_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			if (a_fftshift == 1)
			{
				if (i < (nt_g / 2))
				{

					ftimage2_r_ModS2T[i][j] = ftimage21_r_ModS2T[i + (int)(nt_g / 2) + 1][j];
					ftimage2_i_ModS2T[i][j] = ftimage21_i_ModS2T[i + (int)(nt_g / 2) + 1][j];
				}
				else
				{
					ftimage2_r_ModS2T[i][j] = ftimage21_r_ModS2T[i - (int)(nt_g / 2)][j];
					ftimage2_i_ModS2T[i][j] = ftimage21_i_ModS2T[i - (int)(nt_g / 2)][j];
				}
			}
			if (a_fftshift == 0)
			{
				if (i < (nt_g / 2))
				{
					ftimage2_r_ModS2T[i][j] = ftimage21_r_ModS2T[i + (int)(nt_g / 2)][j];
					ftimage2_i_ModS2T[i][j] = ftimage21_i_ModS2T[i + (int)(nt_g / 2)][j];
				}
				else
				{
					ftimage2_r_ModS2T[i][j] = ftimage21_r_ModS2T[i - (int)(nt_g / 2)][j];
					ftimage2_i_ModS2T[i][j] = ftimage21_i_ModS2T[i - (int)(nt_g / 2)][j];
				}
			}
			// printf("%f  ", ftimage2_r_ModS2T[i][j]);
		}
		//printf("\n");
	}





	free(ftimage21_r_ModS2T); free(ftimage21_i_ModS2T);
	free(ftimage1_r_ModS2T); free(ftimage1_i_ModS2T);
	printf("fin fftshift\n");
	// tau0
	double** ftimage_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	for (int i = 0; i < nt_g; i++)
	{
		ftimage_r_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage_i_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			//teste
			double angle = 2 * pi * (double)(fkz_ModS2T[i][j] - f_ModS2T[i][j]) * tau0;
			double angcos = cos(angle);
			double angsin = -sin(angle);
			double re = ftimage2_r_ModS2T[i][j] * angcos - ftimage2_i_ModS2T[i][j] * angsin;
			double im = ftimage2_i_ModS2T[i][j] * angcos + ftimage2_r_ModS2T[i][j] * angsin;
			ftimage_r_ModS2T[i][j] = re * Filt_r[i][j] - im * Filt_i[i][j];
			ftimage_i_ModS2T[i][j] = re * Filt_i[i][j] + im * Filt_r[i][j];
			//printf("%e\t", ftimage_i_ModS2T[i][j]);
		}
		//printf("\n");

	}
	printf("fin tau0\n");
	free(ftimage2_r_ModS2T); free(ftimage2_i_ModS2T);
	
	//linterp  
	double** Vq_linterp_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** Vq_linterp_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));

	for (int i = 0; i < nt_g; i++)
	{
		Vq_linterp_r_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		Vq_linterp_i_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			Vq_linterp_r_ModS2T[i][j] = 0.0;
			Vq_linterp_i_ModS2T[i][j] = 0.0;
		}
	}
	double dx_linterp_ModS2T = f_ModS2T[1][0] - f_ModS2T[0][0];
	for (int i = 0; i < nu_g; i++)
	{
		for (int j = 0; j < nt_g; j++)
		{
			double f1 = (fkz_ModS2T[j][i] - f_ModS2T[0][0]) / dx_linterp_ModS2T;
			int fMask = !((fkz_ModS2T[j][i] < f_ModS2T[0][0]) | (fkz_ModS2T[j][i] > f_ModS2T[(nt_g - 1)][0]));
			double f = f1 * fMask;
			int im = (int)floor(f);
			int im2Inval = (im + 2 > nt_g - 1) ? 1 : 0;

			double fx = f - (double)im;
			double gx = 1.0 - fx;
			if (fMask == 1)
			{
				if (im2Inval > 0)
				{
					Vq_linterp_r_ModS2T[j][i] = gx * ftimage_r_ModS2T[im][i];
					// printf("0\t");
					Vq_linterp_i_ModS2T[j][i] = gx * ftimage_i_ModS2T[im][i];

				}
				else
				{
					Vq_linterp_r_ModS2T[j][i] = gx * ftimage_r_ModS2T[im][i] + fx * ftimage_r_ModS2T[im + 1][i];
					Vq_linterp_i_ModS2T[j][i] = gx * ftimage_i_ModS2T[im][i] + fx * ftimage_i_ModS2T[im + 1][i];
					// printf("%e\t", ftimage_r_ModS2T[im+1][i]);
				}
			}

		}
		//printf("\n");
	}// fin linterp

	srand(time(NULL));
	printf("fin linterp\n");
	//ifftshift(ftimage)
	double** vq_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** vq_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	for (int i = 0; i < nt_g; i++)
	{
		vq_r_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		vq_i_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{

			if (b_fftshift == 1)
			{
				if (j <= (int)(nu_g / 2))
				{
					vq_r_ModS2T[i][j] = Vq_linterp_r_ModS2T[i][j + (int)(nu_g / 2)];
					vq_i_ModS2T[i][j] = Vq_linterp_i_ModS2T[i][j + (int)(nu_g / 2)];
				}
				else
				{
					vq_r_ModS2T[i][j] = Vq_linterp_r_ModS2T[i][j - ((int)(nu_g / 2) + 1)];
					vq_i_ModS2T[i][j] = Vq_linterp_i_ModS2T[i][j - ((int)(nu_g / 2) + 1)];
				}
			}
			if (b_fftshift == 0)
			{
				if (j < (int)(nu_g / 2))
				{
					vq_r_ModS2T[i][j] = Vq_linterp_r_ModS2T[i][j + (int)(nu_g / 2)];
					vq_i_ModS2T[i][j] = Vq_linterp_i_ModS2T[i][j + (int)(nu_g / 2)];
				}
				else
				{
					vq_r_ModS2T[i][j] = Vq_linterp_r_ModS2T[i][j - (int)(nu_g / 2)];
					vq_i_ModS2T[i][j] = Vq_linterp_i_ModS2T[i][j - (int)(nu_g / 2)];
				}
			}
			//printf("%e\t", vq_r_ModS2T[i][j]);
			//printf("%e\t",vq_i_ModS2T[i][j]);

		}
		//printf("\n");
	}
	// printf("\n");

	double** vq1_r_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** vq1_i_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	for (int i = 0; i < nt_g; i++)
	{
		vq1_r_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		vq1_i_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{

			if (a_fftshift == 1)
			{
				if (i <= (nt_g / 2))
				{

					vq1_r_ModS2T[i][j] = vq_r_ModS2T[i + (int)(nt_g / 2)][j];
					vq1_i_ModS2T[i][j] = vq_i_ModS2T[i + (int)(nt_g / 2)][j];
				}
				else
				{
					vq1_r_ModS2T[i][j] = vq_r_ModS2T[i - ((int)(nt_g / 2) + 1)][j];
					vq1_i_ModS2T[i][j] = vq_i_ModS2T[i - ((int)(nt_g / 2) + 1)][j];
				}
			}
			if (a_fftshift == 0)
			{
				if (i < (nt_g / 2))
				{
					vq1_r_ModS2T[i][j] = vq_r_ModS2T[i + (int)(nt_g / 2)][j];
					vq1_i_ModS2T[i][j] = vq_i_ModS2T[i + (int)(nt_g / 2)][j];
				}
				else
				{
					vq1_r_ModS2T[i][j] = vq_r_ModS2T[i - (int)(nt_g / 2)][j];
					vq1_i_ModS2T[i][j] = vq_i_ModS2T[i - (int)(nt_g / 2)][j];
				}
			}
			//printf("%e\t", vq1_r_ModS2T[i][j]);
			//printf("=%e\n", vq1_i_ModS2T[i][j]);
		}
		//printf("\n");
	}

	free(vq_r_ModS2T); free(vq_i_ModS2T);
	free(Vq_linterp_r_ModS2T); free(Vq_linterp_i_ModS2T);
	// fin nifftshift
	printf("fin ifftshift\n");




	// ifft
	double** HTg_1 = (double**)malloc(M * sizeof(double*));
	double cts_ifft = (nt_g * nu_g);
	for (int n1 = 0; n1 < M; n1++)
	{
		HTg_1[n1] = (double*)malloc(cols * sizeof(double));
		for (int n2 = 0; n2 < cols; n2++)
		{
			double som_r = 0.0;
			for (int k1 = 0; k1 < nt_g; k1++)
			{
				for (int k2 = 0; k2 < nu_g; k2++)
				{
					double angle = 2.0 * pi * ((double)(k1 * n1) / nt_g + (double)(k2 * n2) / nu_g);
					double cosangle = cos(angle);
					double sinangle = sin(angle);
					som_r = som_r + vq1_r_ModS2T[k1][k2] * cosangle - vq1_i_ModS2T[k1][k2] * sinangle;
				}
			}
			HTg_1[n1][n2] = (som_r / cts_ifft);
			//printf("%e\t", HTg_1[n1][n2]);

		}
		// printf("\n");
	}// fin ifft
	clock_t tEnd = clock();
	double time_elapsed = ((double)(tEnd - tAlg)) / CLOCKS_PER_SEC;
	printf("required time of IFFT is  : %f seconds.\n", time_elapsed);


	    save_matrix_as_bmp(HTg_1, cols, M, "HTg_1.bmp");
	    const char* filename4 = "HTg_1.bmp";
	#ifdef _WIN32
	    system("start HTg_1.bmp");
	#elif __APPLE__
	    system("open f_ModS2T.bmp");
	#elif __linux__
	    system("xdg-open f_ModS2T.bmp");
	#else
	    printf("Image sauvegardée sous %s\n", filename4);
	#endif


	double** HTg = (double**)malloc((M * cols) * sizeof(double*));
	for (int i = 0; i < M; i++)
	{
		for (int j = 0; j < cols; j++)
		{
			HTg[((i * cols) + j)] = (double*)malloc(1 * sizeof(double));
			HTg[((i * cols) + j)][0] = HTg_1[i][j] * Rx;
		}
	}
	//free(HTg_1); 
	free(f_ModS2T);
	free(ku_ModS2T); free(fkz_ModS2T);
	free(ftimage_r_ModS2T); free(ftimage_i_ModS2T);
	// fin ModS2T
	free(X_ptAco40dB); free(zz_Load);
	free(ffc_Load); free(scanX);
	free(scanZ); free(roiZ);
	printf("fin ModS2T\n");
	double** fTemp = (double**)malloc((M * cols) * sizeof(double*));
	double** W = (double**)malloc((M * cols) * sizeof(double*));
	double** Y_Model = (double**)malloc((M * cols) * sizeof(double*));
	double** X_cg = (double**)malloc((M * cols) * sizeof(double*));
	for (int i = 0; i < (M * cols); i++)
	{
		fTemp[i] = (double*)malloc(1 * sizeof(double));
		X_cg[i] = (double*)malloc(1 * sizeof(double));
		W[i] = (double*)malloc(1 * sizeof(double));
		Y_Model[i] = (double*)malloc(1 * sizeof(double));
		W[i][0] = 1.0;
		fTemp[i][0] = 0.0; X_cg[i][0] = 0.01;
		Y_Model[i][0] = 0.0;
	}
	// init varaible of model

	double** res_x_szd = (double**)malloc(M * sizeof(double*));
	double** g = (double**)malloc(M * sizeof(double*));
	double** Y1_Model = (double**)malloc(M * sizeof(double*));
	for (int i = 0; i < M; i++)
	{
		res_x_szd[i] = (double*)malloc(cols * sizeof(double));
		g[i] = (double*)malloc(cols * sizeof(double));
		Y1_Model[i] = (double*)malloc(cols * sizeof(double));
		for (int j = 0; j < cols; j++)
		{
			res_x_szd[i][j] = 0.0;
			g[i][j] = 0.0;
			Y1_Model[i][j] = 0.0;
		}

	}

	double** f_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ku_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** fkz_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage1_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage1_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage21_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage21_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage2_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage2_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** V_linterpAdj_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** V_linterpAdj_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** pG_r_linterpAdj_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** pG_i_linterpAdj_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** pF_r_linterpAdj_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** pF_i_linterpAdj_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftimage_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata1_r_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata1_i_ModS2 = (double**)malloc(nt_g * sizeof(double*));
	double** f_model_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ku_model_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** fkz_model_ModS2T = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata1_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata1_i_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata2_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata2_i_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata21_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata21_i_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** ftdata_i_Model = (double**)malloc(nt_g * sizeof(double*));
	double** Vq1_linterp_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** Vq1_linterp_i_Model = (double**)malloc(nt_g * sizeof(double*));
	double** image1_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** image1_i_Model = (double**)malloc(nt_g * sizeof(double*));
	double** image2_r_Model = (double**)malloc(nt_g * sizeof(double*));
	double** image2_i_Model = (double**)malloc(nt_g * sizeof(double*));

	for (int i = 0; i < nt_g; i++)
	{
		f_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ku_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		fkz_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage1_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage1_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage21_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage21_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage2_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage2_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		V_linterpAdj_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		V_linterpAdj_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		pG_r_linterpAdj_ModS2[i] = (double*)malloc(1 * sizeof(double));
		pG_i_linterpAdj_ModS2[i] = (double*)malloc(1 * sizeof(double));
		pF_r_linterpAdj_ModS2[i] = (double*)malloc(1 * sizeof(double));
		pF_i_linterpAdj_ModS2[i] = (double*)malloc(1 * sizeof(double));
		ftimage_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftimage_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata1_r_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata1_i_ModS2[i] = (double*)malloc(nu_g * sizeof(double));
		f_model_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		ku_model_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		fkz_model_ModS2T[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata1_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata1_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata2_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata2_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata21_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata21_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		ftdata_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		Vq1_linterp_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		Vq1_linterp_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		image1_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		image1_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		image2_r_Model[i] = (double*)malloc(nu_g * sizeof(double));
		image2_i_Model[i] = (double*)malloc(nu_g * sizeof(double));
		for (int j = 0; j < nu_g; j++)
		{
			f_ModS2[i][j] = 0.0;
			ku_ModS2[i][j] = 0.0;
			fkz_ModS2[i][j] = 0.0;
			ftimage1_r_ModS2[i][j] = 0.0;
			ftimage1_i_ModS2[i][j] = 0.0;
			ftimage21_r_ModS2[i][j] = 0.0;
			ftimage21_i_ModS2[i][j] = 0.0;
			ftimage2_r_ModS2[i][j] = 0.0;
			ftimage2_i_ModS2[i][j] = 0.0;
			V_linterpAdj_r_ModS2[i][j] = 0.0;
			V_linterpAdj_i_ModS2[i][j] = 0.0;
			ftimage_r_ModS2[i][j] = 0.0;
			ftimage_i_ModS2[i][j] = 0.0;
			ftdata_r_ModS2[i][j] = 0.0;
			ftdata_i_ModS2[i][j] = 0.0;
			ftdata1_r_ModS2[i][j] = 0.0;
			ftdata1_i_ModS2[i][j] = 0.0;
			f_model_ModS2T[i][j] = 0.0;
			ku_model_ModS2T[i][j] = 0.0;
			fkz_model_ModS2T[i][j] = 0.0;
			ftdata1_r_Model[i][j] = 0.0;
			ftdata1_i_Model[i][j] = 0.0;
			ftdata2_r_Model[i][j] = 0.0;
			ftdata2_i_Model[i][j] = 0.0;
			ftdata21_r_Model[i][j] = 0.0;
			ftdata21_i_Model[i][j] = 0.0;
			ftdata_r_Model[i][j] = 0.0;
			ftdata_i_Model[i][j] = 0.0;
			Vq1_linterp_r_Model[i][j] = 0.0;
			Vq1_linterp_i_Model[i][j] = 0.0;
			image1_r_Model[i][j] = 0.0;
			image1_i_Model[i][j] = 0.0;
			image2_r_Model[i][j] = 0.0;
			image2_i_Model[i][j] = 0.0;
		}
		pG_r_linterpAdj_ModS2[i][0] = 0.0;
		pG_i_linterpAdj_ModS2[i][0] = 0.0;
		pF_r_linterpAdj_ModS2[i][0] = 0.0;
		pF_i_linterpAdj_ModS2[i][0] = 0.0;
	}

	// paratmeters of CG

	double** X_yaz = (double**)malloc((cols) * sizeof(double*));
	for (int i = 0; i < ( cols); i++)
	{
		X_yaz[i] = (double*)malloc(cols * sizeof(double));
		for (int j = 0; j < cols; j++)
		{
			X_yaz[i][j] = 0.0;
		}
	}
	double** residual = (double**)malloc((cols) * sizeof(double*));
	double** search_direction = (double**)malloc((cols) * sizeof(double*));
	double** A_search_direction = (double**)malloc((cols) * sizeof(double*));
	for (int i = 0; i < (cols); i++)
	{
		residual[i] = (double*)malloc(cols * sizeof(double));
		search_direction[i] = (double*)malloc(cols * sizeof(double));
		A_search_direction[i] = (double*)malloc(cols * sizeof(double));
		for (int j = 0; j < cols; j++)
		{
			residual[i][j] = 0.0;
			search_direction[i][j] = 0.0;
			A_search_direction[i][j] = 0.0;
		}
	}

	double errUTSR_a = 0;
	int numIter, stagCount, maxIter;
	numIter = 1; stagCount = 0; maxIter = 0;
	// start timing 
	srand(time(NULL));

	printf("start While(1)\n");
	while (1)
	{
		if (numIter > 1)
		{
			if (!isinf(beta))
			{
				while (1)
				{
					double n1fTemp = 0.0; double n1fTempAppr = 0.0; double n1Err = 0.0;
					for (int i = 0; i < (M * cols); i++)
					{
						W[i][0] = 1 / (fabs(fTemp[i][0]) + beta);
						n1fTemp = n1fTemp + fabs(fTemp[i][0]);
						double s = (fabs(sqrt(W[i][0]) * fTemp[i][0]));
						n1fTempAppr = n1fTempAppr + s * s;
					}
					n1Err = (n1fTemp - n1fTempAppr) / n1fTemp;
					if (fabs(n1Err - cg_tol) > cg_tol / 10)
					{
						beta = beta / (n1Err / cg_tol);
					}
					else
					{
						break;
					}
				}
			}
		}
		// Model1(x, W, dt, dx, ERMv, alpha, szData, tau0, Model, Filt, Rx)

		//reshape(x,szData)
		for (int i = 0; i < M; i++)
		{
			for (int j = 0; j < cols; j++)
			{
				res_x_szd[i][j] = X_cg[((j * cols) + i)][0];
			}
		}
		//       save_matrix_as_bmp(res_x_szd, cols, M, "res_x_szd.bmp");
		//    const char* filename6 = "res_x_szd.bmp";
		//#ifdef _WIN32
		//    system("start res_x_szd.bmp");
		//#elif __APPLE__
		//    system("open res_x_szd.bmp");
		//#elif __linux__
		//    system("xdg-open res_x_szd.bmp");
		//#else
		//    printf("Image sauvegardée sous %s\n", filename6);
		//#endif

				////ModS2(reshape(x, szData), dt, dx, ERMv, tau0, Mod, Rx)

				//Data and image grids
		if (numIter == 1)
		{
			double sign_f_ModS2;
			for (int i = 0; i < nt_g; i++)
			{
				for (int j = 0; j < nu_g; j++)
				{
					f_ModS2[i][j] = (((double)(-nt_g / 2.0) + i) / dt) / nt_g;
					ku_ModS2[i][j] = (((double)(-nu_g / 2.0) + j) / du) / nu_g;
					sign_f_ModS2 = (f_ModS2[i][j] > 0) - (f_ModS2[i][j] < 0);
					fkz_ModS2[i][j] = ERMv1 * sign_f_ModS2 * sqrt((ku_ModS2[i][j] * ku_ModS2[i][j]) + ((f_ModS2[i][j] / ERMv1) * (f_ModS2[i][j] / ERMv1)));
				}
			}
		}
		//        save_matrix_as_bmp(fkz_ModS2, nu_g, nt_g, "f_ModS2.bmp");
		//        const char* filename7 = "f_ModS2.bmp";
		//#ifdef _WIN32
		//    system("start f_ModS2.bmp");
		//#elif __APPLE__
		//    system("open f_ModS2.bmp");
		//#elif __linux__
		//    system("xdg-open f_ModS2.bmp");
		//#else
		//    printf("Image sauvegardée sous %s\n", filename7);
		//#endif

				// fft2(res_x_szd, nt, nu)
		double sum_ftimage1_r_ModS2 = 0.0;
		double sum_ftimage1_i_ModS2 = 0.0;
		for (int k1 = 0; k1 < nt_g; k1++)
		{
			for (int k2 = 0; k2 < nu_g; k2++)
			{
				for (int n1 = 0; n1 < M; n1++)
				{
					for (int n2 = 0; n2 < cols; n2++)
					{
						double angle = 2 * pi * ((double)(k1 * n1) / nt_g + (double)(k2 * n2) / nu_g);
						sum_ftimage1_r_ModS2 = sum_ftimage1_r_ModS2 + res_x_szd[n1][n2] * cos(angle);
						sum_ftimage1_i_ModS2 = sum_ftimage1_i_ModS2 - res_x_szd[n1][n2] * sin(angle);
					}
				}
				ftimage1_r_ModS2[k1][k2] = sum_ftimage1_r_ModS2;
				ftimage1_i_ModS2[k1][k2] = sum_ftimage1_i_ModS2;
				sum_ftimage1_i_ModS2 = 0.0;
				sum_ftimage1_r_ModS2 = 0.0;
			}
		}//fin FFT2

		//fftshift
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				if (b_fftshift == 1)
				{
					if (j <= (int)(nu_g / 2))
					{
						ftimage21_r_ModS2[i][j] = ftimage1_r_ModS2[i][j + (int)(nu_g / 2) + 1];
						ftimage21_i_ModS2[i][j] = ftimage1_i_ModS2[i][j + (int)(nu_g / 2) + 1];
					}
					else
					{
						ftimage21_r_ModS2[i][j] = ftimage1_r_ModS2[i][j - (int)(nu_g / 2)];
						ftimage21_i_ModS2[i][j] = ftimage1_i_ModS2[i][j - (int)(nu_g / 2)];
					}
				}
				if (b_fftshift == 0)
				{
					if (j < (int)(nu_g / 2))
					{
						ftimage21_r_ModS2[i][j] = ftimage1_r_ModS2[i][j + (int)(nu_g / 2)];
						ftimage21_i_ModS2[i][j] = ftimage1_i_ModS2[i][j + (int)(nu_g / 2)];
					}
					else
					{
						ftimage21_r_ModS2[i][j] = ftimage1_r_ModS2[i][j - (int)(nu_g / 2)];
						ftimage21_i_ModS2[i][j] = ftimage1_i_ModS2[i][j - (int)(nu_g / 2)];
					}
				}
			}
		}
		for (int i = 0; i < nt_g; i++)
		{

			for (int j = 0; j < nu_g; j++)
			{
				if (a_fftshift == 1)
				{
					if (i <= (nt_g / 2))
					{

						ftimage2_r_ModS2[i][j] = ftimage21_r_ModS2[i + (int)(nt_g / 2) + 1][j];
						ftimage2_i_ModS2[i][j] = ftimage21_i_ModS2[i + (int)(nt_g / 2) + 1][j];
					}
					else
					{
						ftimage2_r_ModS2[i][j] = ftimage21_r_ModS2[i - (int)(nt_g / 2)][j];
						ftimage2_i_ModS2[i][j] = ftimage21_i_ModS2[i - (int)(nt_g / 2)][j];
					}
				}
				if (a_fftshift == 0)
				{
					if (i < (nt_g / 2))
					{
						ftimage2_r_ModS2[i][j] = ftimage21_r_ModS2[i + (int)(nt_g / 2)][j];
						ftimage2_i_ModS2[i][j] = ftimage21_i_ModS2[i + (int)(nt_g / 2)][j];
					}
					else
					{
						ftimage2_r_ModS2[i][j] = ftimage21_r_ModS2[i - (int)(nt_g / 2)][j];
						ftimage2_i_ModS2[i][j] = ftimage21_i_ModS2[i - (int)(nt_g / 2)][j];
					}
				}
			}
		}//fin fftshift

//		save_matrix_as_bmp(ftimage2_r_ModS2, nu_g, nt_g, "ftimage1_r_ModS2.bmp");
//		const char* filename8 = "ftimage1_r_ModS2.bmp";
//#ifdef _WIN32
//		system("start ftimage1_r_ModS2.bmp");
//#elif __APPLE__
//		system("open ftimage1_r_ModS2.bmp");
//#elif __linux__
//		system("xdg-open ftimage1_r_ModS2.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename8);
//#endif

		//linterpAdj(f(:,1), fkz, ftimage);
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				V_linterpAdj_r_ModS2[i][j] = 0.0;
				V_linterpAdj_i_ModS2[i][j] = 0.0;
				pG_r_linterpAdj_ModS2[i][0] = 0.0;
				pG_i_linterpAdj_ModS2[i][0] = 0.0;
				pF_r_linterpAdj_ModS2[i][0] = 0.0;
				pF_i_linterpAdj_ModS2[i][0] = 0.0;
			}
		}
		double dx_linterpAdj_ModS2 = f_ModS2[1][0] - f_ModS2[0][0];
		for (int i = 0; i < nu_g; i++)
		{
			for (int j = 0; j < nt_g; j++)
			{
				double f1_linterpAdj_ModS2 = (fkz_ModS2[j][i] - f_ModS2[0][0]) / dx_linterpAdj_ModS2;
				int fMask_1 = !((fkz_ModS2[j][i] < f_ModS2[0][0]) | (fkz_ModS2[j][i] > f_ModS2[(nt_g - 1)][0]));
				double f_linterpAdj_ModS2 = f1_linterpAdj_ModS2 * fMask_1;
				int im_linterpAdj_ModS2 = (int)floor(f_linterpAdj_ModS2);
				int im2Inval_linterpAdj_ModS2 = (im_linterpAdj_ModS2 + 2 > nt_g) ? 1 : 0;
				double fx_linterpAdj_ModS2 = f_linterpAdj_ModS2 - (double)im_linterpAdj_ModS2;
				double gx_linterpAdj_ModS2 = 1.0 - fx_linterpAdj_ModS2;
				if (fMask_1 == 1)
				{
					pG_r_linterpAdj_ModS2[im_linterpAdj_ModS2][0] = pG_r_linterpAdj_ModS2[im_linterpAdj_ModS2][0] + gx_linterpAdj_ModS2 * ftimage2_r_ModS2[j][i];
					pG_i_linterpAdj_ModS2[im_linterpAdj_ModS2][0] = pG_i_linterpAdj_ModS2[im_linterpAdj_ModS2][0] + gx_linterpAdj_ModS2 * ftimage2_i_ModS2[j][i];
					if (im2Inval_linterpAdj_ModS2 > 0)
					{
						pF_r_linterpAdj_ModS2[nt_g - 1][0] = 0.0;
						pF_i_linterpAdj_ModS2[nt_g - 1][0] = 0.0;
					}
					else
					{
						pF_r_linterpAdj_ModS2[im_linterpAdj_ModS2 + 1][0] = pF_r_linterpAdj_ModS2[im_linterpAdj_ModS2 + 1][0] + fx_linterpAdj_ModS2 * ftimage2_r_ModS2[j][i];
						pF_i_linterpAdj_ModS2[im_linterpAdj_ModS2 + 1][0] = pF_i_linterpAdj_ModS2[im_linterpAdj_ModS2 + 1][0] + fx_linterpAdj_ModS2 * ftimage2_i_ModS2[j][i];
					}
				}
			}
			for (int j = 0; j < nt_g; j++)
			{
				V_linterpAdj_r_ModS2[j][i] = pG_r_linterpAdj_ModS2[j][0] + pF_r_linterpAdj_ModS2[j][0];
				V_linterpAdj_i_ModS2[j][i] = pG_i_linterpAdj_ModS2[j][0] + pF_i_linterpAdj_ModS2[j][0];
				pG_r_linterpAdj_ModS2[j][0] = 0.0;
				pG_i_linterpAdj_ModS2[j][0] = 0.0;
				pF_r_linterpAdj_ModS2[j][0] = 0.0;
				pF_i_linterpAdj_ModS2[j][0] = 0.0;
			}
		}// fin linterpAdj
//		save_matrix_as_bmp(V_linterpAdj_r_ModS2, nu_g, nt_g, "V_linterpAdj_r_ModS2.bmp");
//		const char* filename9 = "V_linterpAdj_r_ModS2.bmp";
//#ifdef _WIN32
//		system("start V_linterpAdj_r_ModS2.bmp");
//#elif __APPLE__
//		system("open V_linterpAdj_r_ModS2.bmp");
//#elif __linux__
//		system("xdg-open V_linterpAdj_r_ModS2.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename9);
//#endif
		// tau0
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				double angle = 2 * pi * (double)(fkz_ModS2[i][j] - f_ModS2[i][j]) * tau0;
				double angcos = cos(angle);
				double angsin = sin(angle);
				double re = V_linterpAdj_r_ModS2[i][j] * angcos - V_linterpAdj_i_ModS2[i][j] * angsin;
				double im = V_linterpAdj_i_ModS2[i][j] * angcos + V_linterpAdj_r_ModS2[i][j] * angsin;
				ftimage_r_ModS2[i][j] = re * Model_r[i][j] - im * Model_i[i][j];
				ftimage_i_ModS2[i][j] = im * Model_r[i][j] + re * Model_i[i][j];
			}
		}

		//		save_matrix_as_bmp(ftimage_r_ModS2, nu_g, nt_g, "ftimage_r_ModS2.bmp");
		//		const char* filename10 = "ftimage_r_ModS2.bmp";
		//#ifdef _WIN32
		//		system("start ftimage_r_ModS2.bmp");
		//#elif __APPLE__
		//		system("open ftimage_r_ModS2.bmp");
		//#elif __linux__
		//		system("xdg-open ftimage_r_ModS2.bmp");
		//#else
		//		printf("Image sauvegardée sous %s\n", filename10);
		//#endif

				//ifftshift(ftdata)
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				if (b_fftshift == 1)
				{
					if (j <= (int)(nu_g / 2))
					{
						ftdata_r_ModS2[i][j] = ftimage_r_ModS2[i][j + (int)(nu_g / 2)];
						ftdata_i_ModS2[i][j] = ftimage_i_ModS2[i][j + (int)(nu_g / 2)];
					}
					else
					{
						ftdata_r_ModS2[i][j] = ftimage_r_ModS2[i][j - ((int)(nu_g / 2) + 1)];
						ftdata_i_ModS2[i][j] = ftimage_i_ModS2[i][j - ((int)(nu_g / 2) + 1)];
					}
				}
				if (b_fftshift == 0)
				{
					if (j < (int)(nu_g / 2))
					{
						ftdata_r_ModS2[i][j] = ftimage_r_ModS2[i][j + (int)(nu_g / 2)];
						ftdata_i_ModS2[i][j] = ftimage_i_ModS2[i][j + (int)(nu_g / 2)];
					}
					else
					{
						ftdata_r_ModS2[i][j] = ftimage_r_ModS2[i][j - (int)(nu_g / 2)];
						ftdata_i_ModS2[i][j] = ftimage_i_ModS2[i][j - (int)(nu_g / 2)];
					}
				}
			}
		}
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				if (a_fftshift == 1)
				{
					if (i <= (nt_g / 2))
					{

						ftdata1_r_ModS2[i][j] = ftdata_r_ModS2[i + (int)(nt_g / 2)][j];
						ftdata1_i_ModS2[i][j] = ftdata_i_ModS2[i + (int)(nt_g / 2)][j];
					}
					else
					{
						ftdata1_r_ModS2[i][j] = ftdata_r_ModS2[i - ((int)(nt_g / 2) + 1)][j];
						ftdata1_i_ModS2[i][j] = ftdata_i_ModS2[i - ((int)(nt_g / 2) + 1)][j];
					}
				}
				if (a_fftshift == 0)
				{
					if (i < (nt_g / 2))
					{
						ftdata1_r_ModS2[i][j] = ftdata_r_ModS2[i + (int)(nt_g / 2)][j];
						ftdata1_i_ModS2[i][j] = ftdata_i_ModS2[i + (int)(nt_g / 2)][j];
					}
					else
					{
						ftdata1_r_ModS2[i][j] = ftdata_r_ModS2[i - (int)(nt_g / 2)][j];
						ftdata1_i_ModS2[i][j] = ftdata_i_ModS2[i - (int)(nt_g / 2)][j];
					}
				}
				//printf("%e\t", ftdata1_r_ModS2[i][j]);
				//printf("=%e\n", vq1_i_ModS2T[i][j]);
			}
			//printf("\n");
		}

		//		save_matrix_as_bmp(ftdata1_r_ModS2, nu_g, nt_g, "ftdata1_r_ModS2.bmp");
		//		const char* filename11 = "ftdata1_r_ModS2.bmp";
		//#ifdef _WIN32
		//		system("start ftdata1_r_ModS2.bmp");
		//#elif __APPLE__
		//		system("open ftdata1_r_ModS2.bmp");
		//#elif __linux__
		//		system("xdg-open ftdata1_r_ModS2.bmp");
		//#else
		//		printf("Image sauvegardée sous %s\n", filename11);
		//#endif

				//ifft2
		cts_ifft = (nt_g * nu_g);
		for (int n1 = 0; n1 < M; n1++)
		{
			for (int n2 = 0; n2 < cols; n2++)
			{
				double som_r = 0.0;
				for (int k1 = 0; k1 < nt_g; k1++)
				{
					for (int k2 = 0; k2 < nu_g; k2++)
					{
						double angle = 2.0 * pi * ((double)(k1 * n1) / nt_g + (double)(k2 * n2) / nu_g);
						double cosangle = cos(angle);
						double sinangle = sin(angle);
						som_r = som_r + ftdata1_r_ModS2[k1][k2] * cosangle - ftdata1_i_ModS2[k1][k2] * sinangle;
					}
				}
				g[n1][n2] = (som_r / cts_ifft) * Rx;
			}
		}// fin ifft
//		save_matrix_as_bmp(g, cols, M, "g.bmp");
//		const char* filename12 = "g.bmp";
//#ifdef _WIN32
//		system("start g.bmp");
//#elif __APPLE__
//		system("open g.bmp");
//#elif __linux__
//		system("xdg-open g.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename12);
//#endif

		//Applies adjoint operator
		//y = ModS2T(g , dt, dx, ERMv, tau0, Filt, Rx);
		//meshgrid(ku,f)
		if (numIter == 1)
		{
			double sign_f;
			for (int i = 0; i < nt_g; i++)
			{
				for (int j = 0; j < nu_g; j++)
				{
					f_model_ModS2T[i][j] = (((double)(-nt_g / 2.0) + i) / dt) / nt_g;
					ku_model_ModS2T[i][j] = (((double)(-nu_g / 2.0) + j) / du) / nu_g;
					sign_f = (f_model_ModS2T[i][j] > 0) - (f_model_ModS2T[i][j] < 0);
					fkz_model_ModS2T[i][j] = ERMv1 * sign_f * sqrt((ku_model_ModS2T[i][j] * ku_model_ModS2T[i][j]) + ((f_model_ModS2T[i][j] / ERMv1) * (f_model_ModS2T[i][j] / ERMv1)));
				}
			}
		}

		//		save_matrix_as_bmp(fkz_model_ModS2T, nu_g, nt_g, "f_model_ModS2T.bmp");
		//		const char* filename13 = "f_model_ModS2T.bmp";
		//#ifdef _WIN32
		//		system("start f_model_ModS2T.bmp");
		//#elif __APPLE__
		//		system("open f_model_ModS2T.bmp");
		//#elif __linux__
		//		system("xdg-open f_model_ModS2T.bmp");
		//#else
		//		printf("Image sauvegardée sous %s\n", filename13);
		//#endif

				//Converting data to frquency domain
				//// FFT2(g, nt_g, nu_g)
		double sum_ftdata1_r_Model = 0.0;
		double sum_ftdata1_i_Model = 0.0;
		for (int k1 = 0; k1 < nt_g; k1++)
		{
			for (int k2 = 0; k2 < nu_g; k2++)
			{
				for (int n1 = 0; n1 < M; n1++)
				{
					for (int n2 = 0; n2 < cols; n2++)
					{
						double angle = 2 * pi * ((double)(k1 * n1) / nt_g + (double)(k2 * n2) / nu_g);
						sum_ftdata1_r_Model = sum_ftdata1_r_Model + g[n1][n2] * cos(angle);
						sum_ftdata1_i_Model = sum_ftdata1_i_Model - g[n1][n2] * sin(angle);
					}
				}
				ftdata1_r_Model[k1][k2] = sum_ftdata1_r_Model;
				ftdata1_i_Model[k1][k2] = sum_ftdata1_i_Model;
				sum_ftdata1_r_Model = 0.0;
				sum_ftdata1_i_Model = 0.0;
			}
		}
		//		save_matrix_as_bmp(ftdata1_r_Model, nu_g, nt_g, "ftdata1_r_Model.bmp");
		//		const char* filename14 = "ftdata1_r_Model.bmp";
		//#ifdef _WIN32
		//		system("start ftdata1_r_Model.bmp");
		//#elif __APPLE__
		//		system("open ftdata1_r_Model.bmp");
		//#elif __linux__
		//		system("xdg-open ftdata1_r_Model.bmp");
		//#else
		//		printf("Image sauvegardée sous %s\n", filename14);
		//#endif

				////fftshift
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{

				if (b_fftshift == 1)
				{
					if (j <= (int)(nu_g / 2))
					{
						ftdata2_r_Model[i][j] = ftdata1_r_Model[i][j + (int)(nu_g / 2) + 1];
						ftdata2_i_Model[i][j] = ftdata1_i_Model[i][j + (int)(nu_g / 2) + 1];
					}
					else
					{
						ftdata2_r_Model[i][j] = ftdata1_r_Model[i][j - (int)(nu_g / 2)];
						ftdata2_i_Model[i][j] = ftdata1_i_Model[i][j - (int)(nu_g / 2)];
					}
				}
				if (b_fftshift == 0)
				{
					if (j < (int)(nu_g / 2))
					{
						ftdata2_r_Model[i][j] = ftdata1_r_Model[i][j + (int)(nu_g / 2)];
						ftdata2_i_Model[i][j] = ftdata1_i_Model[i][j + (int)(nu_g / 2)];
					}
					else
					{
						ftdata2_r_Model[i][j] = ftdata1_r_Model[i][j - (int)(nu_g / 2)];
						ftdata2_i_Model[i][j] = ftdata1_i_Model[i][j - (int)(nu_g / 2)];
					}
				}
			}
		}
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				if (a_fftshift == 1)
				{
					if (i <= (nt_g / 2))
					{

						ftdata21_r_Model[i][j] = ftdata2_r_Model[i + (int)(nt_g / 2) + 1][j];
						ftdata21_i_Model[i][j] = ftdata2_i_Model[i + (int)(nt_g / 2) + 1][j];
					}
					else
					{
						ftdata21_r_Model[i][j] = ftdata2_r_Model[i - (int)(nt_g / 2)][j];
						ftdata21_i_Model[i][j] = ftdata2_i_Model[i - (int)(nt_g / 2)][j];
					}
				}
				if (a_fftshift == 0)
				{
					if (i < (nt_g / 2))
					{
						ftdata21_r_Model[i][j] = ftdata2_r_Model[i + (int)(nt_g / 2)][j];
						ftdata21_i_Model[i][j] = ftdata2_i_Model[i + (int)(nt_g / 2)][j];
					}
					else
					{
						ftdata21_r_Model[i][j] = ftdata2_r_Model[i - (int)(nt_g / 2)][j];
						ftdata21_i_Model[i][j] = ftdata2_i_Model[i - (int)(nt_g / 2)][j];
					}
				}
			}
		}
		// fin fftshift  

//		save_matrix_as_bmp(ftdata21_r_Model, nu_g, nt_g, "ftdata21_r_Model.bmp");
//		const char* filename15 = "ftdata21_r_Model.bmp";
//#ifdef _WIN32
//		system("start ftdata21_r_Model.bmp");
//#elif __APPLE__
//		system("open ftdata21_r_Model.bmp");
//#elif __linux__
//		system("xdg-open ftdata21_r_Model.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename15);
//#endif

		//// tau0
		for (int i = 0; i < nt_g; i++)
		{

			for (int j = 0; j < nu_g; j++)
			{
				double angle = 2 * pi * (double)(fkz_model_ModS2T[i][j] - f_model_ModS2T[i][j]) * tau0;
				double angcos = cos(angle);
				double angsin = -sin(angle);
				double re = ftdata21_r_Model[i][j] * angcos - ftdata21_i_Model[i][j] * angsin;
				double im = ftdata21_i_Model[i][j] * angcos + ftdata21_r_Model[i][j] * angsin;
				ftdata_r_Model[i][j] = re * Filt_r[i][j] - im * Filt_i[i][j];
				ftdata_i_Model[i][j] = re * Filt_i[i][j] + im * Filt_r[i][j];
			}

		}

		//		save_matrix_as_bmp(ftdata_r_Model, nu_g, nt_g, "ftdata_r_Model.bmp");
		//		const char* filename16 = "ftdata_r_Model.bmp";
		//#ifdef _WIN32
		//		system("start ftdata_r_Model.bmp");
		//#elif __APPLE__
		//		system("open ftdata_r_Model.bmp");
		//#elif __linux__
		//		system("xdg-open ftdata_r_Model.bmp");
		//#else
		//		printf("Image sauvegardée sous %s\n", filename16);
		//#endif

				////Generating image spectrum (by linear interpolation)
				//// linterp(f(:,1), ftdata, fkz);
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				Vq1_linterp_r_Model[i][j] = 0.0;
				Vq1_linterp_i_Model[i][j] = 0.0;
			}
		}
		double dx_linterp_Model = f_model_ModS2T[1][0] - f_model_ModS2T[0][0];

		for (int i = 0; i < nu_g; i++)
		{
			for (int j = 0; j < nt_g; j++)
			{
				double f1_model = (fkz_model_ModS2T[j][i] - f_model_ModS2T[0][0]) / dx_linterp_Model;
				int fMask_model = !((fkz_model_ModS2T[j][i] < f_model_ModS2T[0][0]) | (fkz_model_ModS2T[j][i] > f_model_ModS2T[(nt_g - 1)][0]));
				double f_model = f1_model * fMask_model;
				int im_model = (int)floor(f_model);
				int im2Inval_model = (im_model + 2 > nt_g - 1) ? 1 : 0;

				double fx = f_model - (double)im_model;
				double gx = 1.0 - fx;
				if (fMask_model == 1)
				{
					if (im2Inval_model > 0)
					{
						Vq1_linterp_r_Model[j][i] = gx * ftdata_r_Model[im_model][i];
						Vq1_linterp_i_Model[j][i] = gx * ftdata_i_Model[im_model][i];

					}
					else
					{
						Vq1_linterp_r_Model[j][i] = gx * ftdata_r_Model[im_model][i] + fx * ftdata_r_Model[im_model + 1][i];
						Vq1_linterp_i_Model[j][i] = gx * ftdata_i_Model[im_model][i] + fx * ftdata_i_Model[im_model + 1][i];
					}
				}

			}
		}// fin linterp

//		save_matrix_as_bmp(Vq1_linterp_r_Model, nu_g, nt_g, "Vq1_linterp_r_Model.bmp");
//		const char* filename17 = "Vq1_linterp_r_Model.bmp";
//#ifdef _WIN32
//		system("start Vq1_linterp_r_Model.bmp");
//#elif __APPLE__
//		system("open Vq1_linterp_r_Model.bmp");
//#elif __linux__
//		system("xdg-open Vq1_linterp_r_Model.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename17);
//#endif

		// Converting image to space domain
		//ifftshift(Vq1_linterp_r_Model)
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				if (b_fftshift == 1)
				{
					if (j <= (int)(nu_g / 2))
					{
						image1_r_Model[i][j] = Vq1_linterp_r_Model[i][j + (int)(nu_g / 2)];
						image1_i_Model[i][j] = Vq1_linterp_i_Model[i][j + (int)(nu_g / 2)];
					}
					else
					{
						image1_r_Model[i][j] = Vq1_linterp_r_Model[i][j - ((int)(nu_g / 2) + 1)];
						image1_i_Model[i][j] = Vq1_linterp_i_Model[i][j - ((int)(nu_g / 2) + 1)];
					}
				}
				if (b_fftshift == 0)
				{
					if (j < (int)(nu_g / 2))
					{
						image1_r_Model[i][j] = Vq1_linterp_r_Model[i][j + (int)(nu_g / 2)];
						image1_i_Model[i][j] = Vq1_linterp_i_Model[i][j + (int)(nu_g / 2)];
					}
					else
					{
						image1_r_Model[i][j] = Vq1_linterp_r_Model[i][j - (int)(nu_g / 2)];
						image1_i_Model[i][j] = Vq1_linterp_i_Model[i][j - (int)(nu_g / 2)];
					}
				}
			}
		}
		for (int i = 0; i < nt_g; i++)
		{
			for (int j = 0; j < nu_g; j++)
			{
				if (a_fftshift == 1)
				{
					if (i <= (nt_g / 2))
					{

						image2_r_Model[i][j] = image1_r_Model[i + (int)(nt_g / 2)][j];
						image2_i_Model[i][j] = image1_i_Model[i + (int)(nt_g / 2)][j];
					}
					else
					{
						image2_r_Model[i][j] = image1_r_Model[i - ((int)(nt_g / 2) + 1)][j];
						image2_i_Model[i][j] = image1_i_Model[i - ((int)(nt_g / 2) + 1)][j];
					}
				}
				if (a_fftshift == 0)
				{
					if (i < (nt_g / 2))
					{
						image2_r_Model[i][j] = image1_r_Model[i + (int)(nt_g / 2)][j];
						image2_i_Model[i][j] = image1_i_Model[i + (int)(nt_g / 2)][j];
					}
					else
					{
						image2_r_Model[i][j] = image1_r_Model[i - (int)(nt_g / 2)][j];
						image2_i_Model[i][j] = image1_i_Model[i - (int)(nt_g / 2)][j];
					}
				}
			}
		}
		// fin ifftshift

//		save_matrix_as_bmp(image2_r_Model, nu_g, nt_g, "image2_r_Model.bmp");
//		const char* filename18 = "image2_r_Model.bmp";
//#ifdef _WIN32
//		system("start image2_r_Model.bmp");
//#elif __APPLE__
//		system("open image2_r_Model.bmp");
//#elif __linux__
//		system("xdg-open image2_r_Model.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename18);
//#endif

		// ifft
		cts_ifft = (nt_g * nu_g);
		for (int n1 = 0; n1 < M; n1++)
		{
			for (int n2 = 0; n2 < cols; n2++)
			{
				double som_r = 0.0;
				for (int k1 = 0; k1 < nt_g; k1++)
				{
					for (int k2 = 0; k2 < nu_g; k2++)
					{
						double angle = 2.0 * pi * ((double)(k1 * n1) / nt_g + (double)(k2 * n2) / nu_g);
						double cosangle = cos(angle);
						double sinangle = sin(angle);
						som_r = som_r + image2_r_Model[k1][k2] * cosangle - image2_i_Model[k1][k2] * sinangle;
					}
				}
				Y1_Model[n1][n2] = (som_r / cts_ifft) * Rx;
			}
		}// fin ifft

//		save_matrix_as_bmp(Y1_Model, cols, M, "Y1_Model.bmp");
//		const char* filename19 = "Y1_Model.bmp";
//#ifdef _WIN32
//		system("start Y1_Model.bmp");
//#elif __APPLE__
//		system("open Y1_Model.bmp");
//#elif __linux__
//		system("xdg-open Y1_Model.bmp");
//#else
//		printf("Image sauvegardée sous %s\n", filename19);
//#endif

		for (int i = 0; i < M; i++)
		{
			for (int j = 0; j < cols; j++)
			{
				Y_Model[(i * cols) + j][0] = Y1_Model[i][j] + alpha * alpha * W[(i * cols) + j][0] * X_cg[(i * cols) + j][0];
				Y1_Model[i][j]=Y1_Model[i][j] + alpha * alpha * W[(i * cols) + j][0] * X_cg[(i * cols) + j][0];
			}
		}

		// CG 
		int k = 0;
		for (int i = 0; i < cols; i++)
		{
			double sumx, old_resid_norm = 0, sum2;
			for (int i1 = 0; i1 < cols; i1++)
			{
				sumx = 0;
				for (int j = 0; j < cols; j++)
				{
					sumx += Y1_Model[i1][j] *X_yaz[j][i];
				}
				residual[i1][i] = HTg_1[i1][i] - sumx;
				search_direction[i1][i] = residual[i1][i];
				old_resid_norm += residual[i1][i] * residual[i1][i];
			}
			old_resid_norm = sqrt(old_resid_norm);
			
			while (old_resid_norm > 0.0001)
			{
				sum2 = 0;
				for (int i1 = 0; i1 < cols; i1++)
				{
					sumx = 0;
					for (int j = 0; j < cols; j++)
					{
						sumx += Y1_Model[i1][j] * search_direction[j][i];
					}
					A_search_direction[i1][i] = sumx;
					sum2 += A_search_direction[i1][i] * search_direction[i1][i];
				}
				double step_size = old_resid_norm * old_resid_norm / sum2;
				if (step_size < 0)break;
				sumx = 0;
				for (int i1 = 0; i1 < cols; i1++)
				{
					X_yaz[i1][i] += step_size * search_direction[i1][i];
					residual[i1][i] = residual[i1][i] - step_size * A_search_direction[i1][i];
					sumx += residual[i1][i] * residual[i1][i];
				}
				double new_resid_norm = sqrt(sumx);
				for (int i1 = 0; i1 < cols; i1++)
				{
					search_direction[i1][i] = residual[i1][i] + ((new_resid_norm / old_resid_norm) * (new_resid_norm / old_resid_norm)) * search_direction[i1][i];
				}
				//printf("%e ", X_yaz[0][i]);
				old_resid_norm = new_resid_norm;
				k = k + 1;
				

			}
			printf("%d \n", i);
		}
		printf("k=%d\n", k);


		for (int i = 0; i < cols; i++)
		{
			for (int j = 0; j < cols; j++)
			{
				//X_cg[i*cols+j][0] = X_yaz[i][j];
				if (X_cg[i * cols + j][0] < 0)
				{
					X_cg[i * cols + j][0] = 0.0;
				}
			}
			
		}
		break;

	}
	//Reshape found solution as a matrix
	// outputRaw = reshape(f, szData);
	double** outputRaw = (double**)malloc(M * sizeof(double*));
	for (int i = 0; i < M; i++)
	{
		outputRaw[i] = (double*)malloc(cols * sizeof(double));
		for (int j = 0; j < cols; j++)
		{
			outputRaw[i][j] = X_cg[i * cols + j][0];
			//printf("%e ", X_yaz[i][j]);
		}
		//printf("\n");
	}
	free(Vq1_linterp_r_Model); free(Vq1_linterp_i_Model);
	free(HTg); free(fTemp); free(W); free(g); free(Y1_Model);
	free(Y_Model); free(ku_ModS2); free(res_x_szd);
	free(ftimage1_r_ModS2); free(ftimage1_i_ModS2);
	free(ftimage21_r_ModS2); free(ftimage21_i_ModS2);
	free(ftimage2_r_ModS2); free(ftimage2_i_ModS2);
	free(pG_r_linterpAdj_ModS2); free(pG_i_linterpAdj_ModS2);
	free(pF_r_linterpAdj_ModS2); free(pF_i_linterpAdj_ModS2);
	free(V_linterpAdj_r_ModS2); free(V_linterpAdj_i_ModS2);
	free(ftimage_r_ModS2); free(ftimage_i_ModS2);
	free(fkz_ModS2); free(f_ModS2);
	free(ftdata_r_ModS2); free(ftdata_i_ModS2);
	free(ftdata1_r_ModS2); free(ftdata1_i_ModS2);
	free(ku_model_ModS2T); free(ftdata1_i_Model); free(ftdata1_r_Model);
	free(ftdata2_r_Model); free(ftdata2_i_Model);
	free(ftdata21_r_Model); free(ftdata21_i_Model);
	free(ftdata_r_Model); free(ftdata_i_Model);
	free(f_model_ModS2T); free(fkz_model_ModS2T);
	free(image1_r_Model); free(image1_i_Model);
	free(image2_r_Model); free(image2_i_Model);
	tEnd = clock();
	time_elapsed = ((double)(tEnd - tAlg)) / CLOCKS_PER_SEC;
		printf("Elapsed time is : %f seconds.\n", time_elapsed);
		save_matrix_as_bmp(outputRaw, cols, M, "output.bmp");
		const char* filename = "output.bmp";
	#ifdef _WIN32
		system("start output.bmp");
	#elif __APPLE__
		system("open output.bmp");
	#elif __linux__
		system("xdg-open output.bmp");
	#else
		printf("Image sauvegardée sous %s\n", filename);
	#endif
		free(outputRaw);
	free(Model_r); free(Model_i);
	free(Filt_r); free(Filt_i);
	free(AscanValues_ptAco40dB); free(timeScale_ptAco40dB);
	free(S_t_u);

}



int nextpow2(int n) {
	int exp = 0;
	if (n < 1) return 0;
	n--;
	while (n > 0) {
		n >>= 1;
		exp++;
	}
	return exp;
}

double sinc(double x) {
	if (x == 0) {
		return 1.0;
	}
	else {
		return sin(pi * x) / (pi * x);
	}
}

//double** X_pcg = (double**)malloc((M * cols) * sizeof(double*));
//double** xmin_pcg = (double**)malloc((M * cols) * sizeof(double*));
//double** r_pcg = (double**)malloc((M * cols) * sizeof(double*));
//double** y_pcg = (double**)malloc((M * cols) * sizeof(double*));
//double** z_pcg = (double**)malloc((M * cols) * sizeof(double*));
//double** p_pcg = (double**)malloc((M * cols) * sizeof(double*));
//double** q_pcg = (double**)malloc((M * cols) * sizeof(double*));

////
////
//// PCG
////int flag_pcg, iter_pcg;
////int while_2 = 0;
////while (while_2 == 0)
////{
////	printf("pcg\n");
////	while_2 += 1;
////	 Preconditioned Conjugate Gradient method 
////	 [f, flag, ~, iter] = pcg(yazHTg(:), cg_tol, M*N, [], [], fTemp);
////	 iterchk(A);
////	 afun = Y_Model;
////	int m_pcg = M * cols, n_pcg = 1;
////	Assign default values to unspecified parameters
////	int warned_pcg = 0, maxit_pcg;
////	maxit_pcg = M * cols;
////	if (maxit_pcg < 0)
////	{
////		maxit_pcg = 0;
////	}
////	Check for all zero right hand side vector => all zero solution
////	n2b = norm(HTg(:));                     % Norm of rhs vector, b
////	double n2b = 0.0;
////	for (int i = 0; i < (M * cols); i++)
////	{
////		n2b = n2b + HTg[i][0] * HTg[i][0];
////	}
////	n2b = sqrt(n2b);
////	if (n2b == 0)
////	{
////		flag_pcg = 0;
////		iter_pcg = 0;
////		for (int i = 0; i < (M * cols); i++)
////		{
////			X_pcg[i][0] = 0.0;
////		}
////		break;
////	}
////
////	for (int i = 0; i < (M * cols); i++)
////	{
////		X_pcg[i][0] = fTemp[i][0];
////	}
////	Set up for the method
////	flag_pcg = 1;
////	double tolb_pcg = cg_tol * n2b;
////	double normr_pcg = 0.0, normr_act_pcg;
////	int imin_pcg = 0;
////	for (int i = 0; i < (M * cols); i++)
////	{
////		xmin_pcg[i][0] = X_pcg[i][0];
////	}
////	 r = b - iterapp('mtimes',afun,atype,x,varargin{:});
////	for (int i = 0; i < (M * cols); i++)
////	{
////		r_pcg[i][0] = HTg[i][0] - (Y_Model[i][0] * X_pcg[i][0]);
////		normr_pcg = r_pcg[i][0] * r_pcg[i][0] + normr_pcg;
////	}
////	normr_pcg = sqrt(normr_pcg);
////	normr_act_pcg = normr_pcg;
////	 Initial guess is a good enough solution
////	if (normr_pcg <= tolb_pcg)
////	{
////		flag_pcg = 0;
////		iter_pcg = 0;
////		break;
////	}
////	double normrmin_pcg = normr_pcg, rho_pcg;
////	int stag_pcg = 0, moresteps_pcg = 0, maxmsteps_pcg, maxstagsteps_pcg;
////	rho_pcg = 1.0;
////	maxmsteps_pcg = 1 - (M * cols);
////	if (maxmsteps_pcg > 0)
////	{
////		maxmsteps_pcg = 0;
////	}
////	maxstagsteps_pcg = 3;
////	double pq_pcg, alpha_pcg = 0.0;
////	loop over maxit iterations (unless convergence or failure)
////	int rho1_pcg;
////	double beta_pcg, normp_pcg, normx_pcg;
////	int ii;
////	for (ii = 0; ii < (M * cols); ii++)
////	{
////		for (int i = 0; i < (M * cols); i++)
////		{
////			y_pcg[i][0] = r_pcg[i][0];
////			z_pcg[i][0] = y_pcg[i][0];
////		}
////		rho1_pcg = rho_pcg;
////		if ((rho_pcg == 0) || isinf(rho_pcg))
////		{
////			flag_pcg = 4;
////			break;
////		}
////
////		if (ii == 0)
////		{
////			for (int i = 0; i < (M * cols); i++)
////			{
////				p_pcg[i][0] = z_pcg[i][0];
////			}
////		}
////		else
////		{
////			beta_pcg = ((double)rho_pcg) / ((double)rho1_pcg);
////			if ((beta_pcg == 0) || isinf(beta_pcg))
////			{
////				flag_pcg = 4;
////				break;
////			}
////			for (int i = 0; i < (M * cols); i++)
////			{
////				p_pcg[i][0] = z_pcg[i][0] + beta_pcg * p_pcg[i][0];
////			}
////		}
////		pq_pcg = 0.0;
////		normp_pcg = 0.0;
////		normx_pcg = 0.0;
////		for (int i = 0; i < (M * cols); i++)
////		{
////			q_pcg[i][0] = (Y_Model[i][0] * p_pcg[i][0]);
////			pq_pcg = q_pcg[i][0] * p_pcg[i][0] + pq_pcg;
////			normp_pcg = p_pcg[i][0] * p_pcg[i][0] + normp_pcg;
////			normx_pcg = normx_pcg + X_pcg[i][0] * X_pcg[i][0];
////		}
////		if ((pq_pcg <= 0) || isinf(pq_pcg))
////		{
////			flag_pcg = 4;
////			break;
////		}
////		else
////		{
////			alpha_pcg = (double)rho_pcg / pq_pcg;
////		}
////		if (isinf(alpha_pcg))
////		{
////			flag_pcg = 4;
////			break;
////		}
////
////		 Check for stagnation of the method
////		normp_pcg = sqrt(normp_pcg);
////		normx_pcg = sqrt(normx_pcg);
////		if (normp_pcg * fabs(alpha_pcg) < eps * normx_pcg)
////		{
////			stag_pcg += 1;
////		}
////		else
////		{
////			stag_pcg = 0;
////		}
////		form new iterate
////		normr_pcg = 0.0;
////		for (int i = 0; i < (M * cols); i++)
////		{
////			X_pcg[i][0] += alpha_pcg * p_pcg[i][0];
////			r_pcg[i][0] = r_pcg[i][0] - alpha_pcg * q_pcg[i][0];
////			normr_pcg += r_pcg[i][0] * r_pcg[i][0];
////		}
////		normr_pcg = sqrt(normr_pcg);
////		normr_act_pcg = normr_pcg;
////
////		 check for convergence
////
////		if (normr_pcg <= tolb_pcg || stag_pcg >= maxstagsteps_pcg || moresteps_pcg)
////		{
////			r = b - iterapp('mtimes',afun,atype,x,varargin{:});
////			normr_pcg = 0.0;
////			for (int i = 0; i < (M * cols); i++)
////			{
////				r_pcg[i][0] = HTg[i][0] - (Y_Model[i][0] * X_pcg[i][0]);
////				normr_pcg = r_pcg[i][0] * r_pcg[i][0] + normr_pcg;
////			}
////			normr_pcg = sqrt(normr_pcg);
////			normr_act_pcg = normr_pcg;
////			if (normr_act_pcg <= tolb_pcg)
////			{
////				flag_pcg = 0;
////				iter_pcg = ii;
////				break;
////			}
////			else
////			{
////				if (stag_pcg >= maxstagsteps_pcg && moresteps_pcg == 0)
////				{
////					stag_pcg = 0;
////				}
////				moresteps_pcg += 1;
////				if (moresteps_pcg >= maxmsteps_pcg)
////				{
////					flag_pcg = 3;
////					iter_pcg = ii;
////					break;
////				}
////			}
////		}
////
////		update minimal norm quantities
////		if (normr_act_pcg < normrmin_pcg)
////		{
////			normrmin_pcg = normr_act_pcg;
////			imin_pcg = ii;
////			for (int i = 0; i < (M * cols); i++)
////			{
////				xmin_pcg[i][0] = X_pcg[i][0];
////			}
////
////		}
////		if (stag_pcg >= maxstagsteps_pcg)
////		{
////			flag_pcg = 3;
////			break;
////		}
////
////	}
////	// returned solution is first with minimal residual
////	double r_comp_pcg = 0.0;
////	if (flag_pcg != 0)
////	{
////		for (int i = 0; i < (M * cols); i++)
////		{
////			r_comp_pcg += (HTg[i][0] - (Y_Model[i][0] * xmin_pcg[i][0])) * (HTg[i][0] - (Y_Model[i][0] * xmin_pcg[i][0]));
////		}
////		r_comp_pcg = sqrt(r_comp_pcg);
////		if (r_comp_pcg <= normr_act_pcg)
////		{
////			for (int i = 0; i < (M * cols); i++)
////			{
////				X_pcg[i][0] = xmin_pcg[i][0];
////			}
////			iter_pcg = imin_pcg;
////		}
////		else
////		{
////			iter_pcg = ii;
////		}
////	}
////}// fin PCG
////printf("cg_tol = %f | iter = %d | flag = %d \n", cg_tol, iter_pcg, flag_pcg);