/* math.c
   ------
   Calculates path loss exponent and partition model coefficients for a given set of Cell information
   
   Uses:
   GSL (Gnu Scientific Library)      - to calculate normal equations with matrices
   Matlab External Interface Library - to load and save .MAT data
*/

#include "include\mat.h"
#include <gsl\gsl_linalg.h>
#include <gsl\gsl_blas.h>
#include "stdlib.h"
#include "conio.h"
#include "math.h"
#include "string.h"

#define PI 3.1415926535897932384626433832795

/* Matlab file locations + pragmas */
#pragma comment(lib, "C:\\Appl\\MatlabR14\\extern\\lib\\win32\\microsoft\\msvc60\\libmat.lib")
#pragma comment(lib, "C:\\Appl\\MatlabR14\\extern\\lib\\win32\\microsoft\\msvc60\\libmx.lib")
#pragma comment(lib, "C:\\Appl\\MatlabR14\\extern\\lib\\win32\\microsoft\\msvc60\\libeng.lib")
#pragma comment(lib, "libgsl.lib")
#define CELL_INFO_FILE "C:\\temp\\3065\\cell_info.mat"

/* The Cell_Info_# variable to analyze */
#define CELL_VAR_NAME "Cell_Info_A"

/* blockedDistance - calculates how much space between (tx, ty, tz) & (rx, ry, rz) is blocked */
int numBlocks(short *terrain, int tx, int ty, int tz, int rx, int ry, int rz, int h, int cellSize, double wavelength, double *diff_score);
double simulate_rps(char *sim, char *meas, short *terrain, int w, int h, double n, double *coeffs, int tx, int ty, int cellSize, double azimuth, double wavelength);
double simulate_rps_exp(char *sim, char *meas, short *terrain, int w, int h, double n, double *coeffs, int tx, int ty, int cellSize, double path_loss_const);

/* Partition variables... these will go in a matlab array */
typedef struct partition_vars_struct {
	double distance;
	double is_blocked;
	double excess_path_loss;
	double blocked_distance;
} partition_vars;

typedef struct partition_struct {
	double distance;
	double blocks;
	double excess_path_loss;
	double angle;
	double diff_score;
	double in_azimuth;
} partition;

/* GSL matrix operation wrapper functions */
gsl_matrix *matrix_multiply(gsl_matrix *a, gsl_matrix *b);
gsl_matrix *matrix_inverse(gsl_matrix *a);
gsl_matrix *matrix_transpose(gsl_matrix *a);
double *matrix_multiply_vector(gsl_matrix *a, double *x);
void matrix_printf (gsl_matrix *a);
double sinc(double a);

int main(int argc, char* argv[])
{
	// Matlab objects
	MATFile *pmat;
    mxArray *cell_info;
	mxArray *meas_array;
	mxArray *terrain_array;
	mxArray *cell_site;
	mxArray *azimuth_array;
	mxArray *eirp_array;
	mxArray *BS_x_array;
	mxArray *BS_y_array;
	mxArray *freq_array;
	mxArray *cellSize_array;
	mxArray *height_array;

	// Variables used in calculations - da-dang-da-dang.
	char   *meas;
	short  *terrain;
	const int *meas_dim;
	double azimuth;
	double eirp;
	double BS_x;
	double BS_y;
	double freq;
	double cellSize;
	double height;
	int w;
	int h;
	int rc; // receiver count
	int x, y, i;
	double z, r;
	double d, dx, dy, dz;
	double tx, ty, tz;
	double lr;
	double numer, denom;
	double n;
	double path_loss_const;
	double wavelength;
	char *sim;
	double avg_err;
	double azimuth_x;
	double azimuth_y;
	double azimuth_rad;
	double ang2d_rad;
	double ang2d_x;
	double ang2d_y;
	double azimuth_da;
	double angular_spread;
	partition *stats;
	double *received_power;

	/* matrices used in the normal equations */
	gsl_matrix *pm;
	gsl_matrix *pm_transpose;
	gsl_matrix *pm_ata;
	gsl_matrix *pm_ata_inv;
	gsl_matrix *pm_ata_at;
	double *pm_coeffs;

    /* open mat file and read it's content */
	printf("Loading data");
    pmat = matOpen(CELL_INFO_FILE, "r");
	if (!pmat) {
		printf("Couldn't load file\n");
		return 0;
	}

	/* get cell info arrays */
	printf(".");
	cell_info = matGetVariable(pmat, CELL_VAR_NAME);
	printf(".");
	meas_array = mxGetField(cell_info, 0, "meas");
	terrain_array = mxGetField(cell_info, 0, "terrain");
	cell_site = mxGetField(cell_info, 0, "cellSite");
	azimuth_array = mxGetField(cell_site, 0, "azimuth");
	eirp_array = mxGetField(cell_site, 0, "eirp");
	BS_x_array = mxGetField(cell_site, 0, "BSx");
	BS_y_array = mxGetField(cell_site, 0, "BSy");
	freq_array = mxGetField(cell_site, 0, "freq");
	cellSize_array = mxGetField(cell_site, 0, "cellSize");
	height_array = mxGetField(cell_site, 0, "height");
	meas_dim = mxGetDimensions(meas_array);
	w = meas_dim[1];
	h = meas_dim[0];

	/* create partition array */
	stats = calloc(w*h, sizeof(partition));
	received_power = calloc(w*h, sizeof(partition));

	/* get cell info data */
	meas = (char *)mxGetPr(meas_array);
	terrain =(short *)mxGetPr(terrain_array);
	azimuth = mxGetScalar(azimuth_array);
	eirp = mxGetScalar(eirp_array);
	BS_x = mxGetScalar(BS_x_array);
	BS_y = mxGetScalar(BS_y_array);
	freq = mxGetScalar(freq_array);
	cellSize = mxGetScalar(cellSize_array);
	height = mxGetScalar(height_array);
	printf("done\n");

	/* display terrain info */
	printf("\nMap Information:\n");
	printf("Name: %s\n", CELL_NAME);
	printf("Width: %d\n", w);
	printf("Height: %d\n", h);
	printf("\n");

	/* calculate partition stats */
	printf("Compiling partition statistics");
	rc = 0;
	tx = BS_x-1;
	ty = BS_y-1;
	tz = terrain[(int)tx*h+(int)ty] + height;
	wavelength = 3*pow(10,8) / (freq*1000000);

	/* preliminary azimuth angle stats */
	azimuth_rad = (-azimuth+90) * PI / 180;
	azimuth_x = cos(azimuth_rad);
	azimuth_y = sin(azimuth_rad);
	angular_spread = 120*PI/180;//sqrt( 1-sinc( azimuth_rad / (2*PI) ) );

	/* path loss "constant" - used to find path loss */
	path_loss_const = eirp + 20*log10(wavelength / (4*PI));
	numer = 0;
	denom = 0;

	/* use each measurement to calculate partitions and the best-fitting path loss */
	for (y=0; y<h; y++) {
		for (x=0; x<w; x++) {
			if (meas[x*h+y] != 0) {
				z = terrain[x*h+y];
				stats[rc].excess_path_loss = 1;

				/* calculate distance */
				dx = ((double)x - tx)*cellSize;
				dy = ((double)y - ty)*cellSize;
				dz = z - tz;
				d = sqrt(dx*dx + dy*dy + dz*dz);
				r = sqrt(dx*dx + dy*dy);

				/* get various stats for partition model */
				stats[rc].distance = d;
				stats[rc].blocks = numBlocks(terrain, (int)tx, (int)ty, (int)tz, x, y, (int)z, h, (int)cellSize, wavelength, &stats[rc].diff_score);
				stats[rc].angle = atan2(dz, r)*180/PI;

				/* determine if in azimuth angular spread */
				ang2d_rad = atan2(dy, dx);
				ang2d_x = cos(ang2d_rad);
				ang2d_y = sin(ang2d_rad);
				azimuth_da = acos(ang2d_x*azimuth_x + ang2d_y*azimuth_y);

				if (azimuth_da < (angular_spread/2)) stats[rc].in_azimuth = 1;
				else stats[rc].in_azimuth = 0;
				//stats[rc].in_azimuth = azimuth_da;

				/* record b vector */
				received_power[rc] = (double)meas[x*h+y];
				lr = log10(d);

				numer += lr * (path_loss_const - received_power[rc]);
				denom += lr*lr;
				
				rc++;				
			}
			i++;
		}

		if (!(y % 100)) printf(".");
	}
	n = numer / denom / 10;
	printf("done\n");
	printf("\nPath loss exponent: %2.4f\n\n", n);


	/* calculate partition coefficients */
	pm = gsl_matrix_alloc(rc, 6);
	memcpy(pm->data, stats, rc*6*sizeof(double));
	pm_transpose = matrix_transpose(pm);
	pm_ata = matrix_multiply(pm_transpose, pm);
	pm_ata_inv = matrix_inverse(pm_ata);
	pm_ata_at = matrix_multiply(pm_ata_inv, pm_transpose);
	pm_coeffs = matrix_multiply_vector(pm_ata_at, received_power);

	/* print out coefficients */
	printf("done\n");
	printf("\nPartition model coefficients:\n");
	printf("Distance coeff:        %2.5e\n", pm_coeffs[0]);
	printf("L.O.S coeff:           %2.5e\n", pm_coeffs[1]);
	printf("Excess coeff:          %2.5e\n", pm_coeffs[2]);
	printf("Elevation angle coeff: %2.5e\n", pm_coeffs[3]);
	printf("Diffraction coeff:     %2.5e\n", pm_coeffs[4]);
	printf("Azimuth angle coeff:   %2.5e\n", pm_coeffs[5]);
	printf("\n");
	
	/* Perform a quick simulation to estimate average error */
	printf("Simulating measurements");

	sim = calloc(w*h*sizeof(char), 1);
	avg_err = simulate_rps(sim, meas, terrain, w, h, n, pm_coeffs, (int)tx, (int)ty, (int)cellSize, azimuth, wavelength);
	printf("done\n");
	printf("\nAverage error: %2.2f", avg_err);
	
	// Close up
	gsl_matrix_free(pm);
	gsl_matrix_free(pm_transpose);
	gsl_matrix_free(pm_ata);
	gsl_matrix_free(pm_ata_inv);
	gsl_matrix_free(pm_ata_at);
	
	free(received_power);
	free(pm_coeffs);
	free(stats);

	mxDestroyArray(cell_info);
	matClose(pmat);	
	
	printf("\nFinished!\n");

	getch();
	
	return 0;
}

/* numBlocks - calculates how much space between (tx, ty, tz) & (rx, ry, rz) is blocked */
int numBlocks(short *terrain, int tx, int ty, int tz, int rx, int ry, int rz, int h, int cellSize, double wavelength, double *diff_score) {
	double distance2d;
	double angle2d;
	double zslope;
	double expected_z;
	double actual_z;static count=0;
	
	int thisx, thisy;
	int r;
	int blocks=0;
	double max_ob_h = 0;

	double ob_h, d1, d2, v;

	int dx = rx - tx;
	int dy = ry - ty;
	int dz = rz - tz;
	
	distance2d = sqrt(dx*dx+dy*dy);
	angle2d = atan2(dy, dx);
	zslope = (double)dz / distance2d;
	*diff_score = 0;

	for (r=0; r<(int)distance2d; r+=cellSize) {
		thisx = (int)(cos(angle2d)*r)+tx;
		thisy = (int)(sin(angle2d)*r)+ty;

		expected_z = zslope*r + tz;
		actual_z = (double)terrain[thisx*h+thisy];

		if (actual_z > expected_z) {
			/* compute diffraction score */
				if ( abs((int)actual_z - tz) < (int)(distance2d / 100)) {
					ob_h = actual_z - rz;
					d1 = r;
					d2 = distance2d - r;
					v = ob_h*(d1+d2)/(wavelength*d1*d2);
					count++;
					if (ob_h > max_ob_h) {
						*diff_score = v;
						max_ob_h = ob_h;
					}
				}


			blocks += 1;
		}
	}

	return (blocks > 0);
}

// matrix_multiply(a, b) - return c = a*b
gsl_matrix *matrix_multiply(gsl_matrix *a, gsl_matrix *b) {
	gsl_matrix *c;
	int i, j, k;
	int rows_a = a->size1;
	int rows_b = b->size1;
	int rows_c = rows_a;
	int cols_a = a->size2;	
	int cols_b = b->size2;
	int cols_c = cols_b;
	
	if ((rows_b) != (cols_a)) return 0;

	c = gsl_matrix_alloc(rows_c, cols_c);
	if (!c) return 0;

	for (i=0; i<cols_c; i++) {
		for (j=0; j<rows_c; j++) {
			c->data[j*cols_c+i] = 0;
			for (k=0; k<rows_b; k++) {
				c->data[j*cols_c+i] += (double)a->data[j*cols_a+k] * (double)b->data[k*cols_b+i];
			}
		}
	}
		
	return c;
}

/* matrix_inverse(a) - returns c = inv(a) from matlab */
gsl_matrix *matrix_inverse(gsl_matrix *a) {
	gsl_matrix *b;
	gsl_matrix *c;
	gsl_permutation *perm = gsl_permutation_alloc (a->size1);
	int s;

	b = gsl_matrix_alloc(a->size1, a->size2);
	c = gsl_matrix_alloc(a->size1, a->size2);
	memcpy(b->data, a->data, a->size1*a->size2*sizeof(double));

	gsl_linalg_LU_decomp (b, perm, &s);
	gsl_linalg_LU_invert (b, perm, c);

	gsl_permutation_free(perm);
	gsl_matrix_free(b);

	return c;
}

/* matrix_transpose(a) - returns b = transpose(a) from matlab */
gsl_matrix *matrix_transpose(gsl_matrix *a) {
	gsl_matrix *b;
	int rows = a->size1;
	int cols = a->size2;
	int brows = cols;
	int bcols = rows;

	b = gsl_matrix_alloc(brows, bcols);
	gsl_matrix_transpose_memcpy(b, a);

	return b;
}

/* matrix_multiply_vector(a, x) - returns b = A*x */
double *matrix_multiply_vector(gsl_matrix *a, double *x) {
	int i;
	int j;
	double *result;
	int rows = a->size1;
	int cols = a->size2;

	result = malloc(a->size2 * sizeof(double));
	for (j=0; j<rows; j++) {
		result[j]=0;
		for (i=0; i<cols; i++) {
			result[j] += x[i]*a->data[j*a->size2+i];
		}
	}

	return result;
}

/* simulate_rps - finds average error using a partion model */
double simulate_rps(char *sim, char *meas, short *terrain, int w, int h, double n, double *coeffs, int tx, int ty, int cellSize, double azimuth, double wavelength) {
	int x, y, i, b;
	double dx, dy, dz, d;
	double tz, z;
	double r;
	double sum = 0;
	double avg_err;
	double a;
	double diff_score;
	int rc=0;

	double ang2d_rad, ang2d_x, ang2d_y;
	double azimuth_rad, azimuth_x, azimuth_y;
	double azimuth_da, angular_spread;

	int in_azimuth;

	i = 0;
	tz = terrain[tx*h+ty];

	azimuth_rad = (-azimuth+90) * PI / 180;
	azimuth_x = cos(azimuth_rad);
	azimuth_y = sin(azimuth_rad);
	angular_spread = 120*PI/180;  //sqrt( 1-sinc( azimuth_rad / (2*PI) ) );

	for (x=0; x<w; x++) {
		for (y=0; y<h; y++) {
			/* calculate distance */
			z = terrain[i];
			dx = ((double)x - tx)*cellSize;
			dy = ((double)y - ty)*cellSize;
			dz = z - tz;
			d = sqrt(dx*dx + dy*dy + dz*dz);
			r = sqrt(dx*dx + dy*dy);
			a = atan2(dz, r)*180/PI;
			
			/* determine if receiver is inside azimuth angle spread */
			ang2d_rad = atan2(dy, dx);
			ang2d_x = cos(ang2d_rad);
			ang2d_y = sin(ang2d_rad);
			azimuth_da = acos(ang2d_x*azimuth_x + ang2d_y*azimuth_y);
			if (azimuth_da < (angular_spread/2)) in_azimuth = 1;
			else in_azimuth = 0;

			/* Find stats and compare measurements */
			if (meas[i]) {
				b = numBlocks(terrain, tx, ty, (int)tz, x, y, (int)z, h, (int)cellSize, wavelength, &diff_score);
				sim[i] = (char)(d*coeffs[0] + b*coeffs[1] + coeffs[2] + a*coeffs[3] + diff_score*coeffs[4] + in_azimuth*coeffs[5]);
				if (sim[i] < -126) sim[i] = -126;

				sum += abs(meas[i] - sim[i]);
				rc++;
			}

			i++;
		}

		if (!(x % 100)) printf(".");
	}

	avg_err = sum / rc;
	return avg_err;
}

/* matrix_printf - prints out a GSL matrix */
void matrix_printf (gsl_matrix *a) {
	int rows = (int)a->size1;
	int cols = (int)a->size2;
	int x, y;

	for (y = 0; (y<rows) && (y<20); y++) {
		for (x=0; (x<cols) && (x<5); x++) {
			printf("%2.2f   ", (double)a->data[y*cols+x]);
		}
		if (cols > 10) printf("...");
		printf("\n");
	}
	if (rows > 20) printf("...");
	printf("\n");
}

/* sinc(a) - the sinc function */
double sinc(double a) {
	if (a == 0) return 1;
	else return sin(a) / a;
}