/***************************************************************************
Author: Ian C. Wong and Robert Mullenix   
Copyright (C) 2004   Ian C. Wong

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.

You may reach the author at wongic@mail.utexas.edu.
Or visit his website at www.ece.utexas.edu/~iwong
******************************************************************************/
#include "SIPS.h"
//  Performs Shen's Power Allocation
int Wong_Palloc(int user, SNRArray *SNR, int *Wong_Assignment, float *Data_rate, 
				int *Hmin)
{
	int k;
	int n;
	int error = 0;
	float ptemp;
	//float sum;							//  Number of iterations used in the Newton-Raphson method
	//float sum2;							//  Number of iterations used in the Newton-Raphson method
	//float fx;							//  Solution of equation (17) in Shen's paper, with P1,tot guessed
	//float dfx;							//  Soltuion to the derivative of equation (17) in Shen's paper, with P1, tot guessed
	
	float *User_pow;	
	int *Num_chan;	
	double *Gmean;
	float *Hmean;
	float *a;
	float *b;
	float *ab;
	float gm0, gmk;
	float pow_num = (float) tot_Power;
	float pow_den = 1.0f;

	User_pow = (float *) malloc(user*sizeof(float));
	Num_chan = (int *) malloc(user*sizeof(int));
	Gmean = (double *) malloc(user*sizeof(double));	
	Hmean = (float *) malloc(user*sizeof(float));
	a = (float *) malloc(user*sizeof(float));
	b = (float *) malloc(user*sizeof(float));
	ab = (float *) malloc(user*sizeof(float));		
	
	for (k = 0; k < user; k++)	{		
		Num_chan[k] = 0;
		Gmean[k] = 1.0;
		Hmean[k] = 0.0;
	}
	for (n = 0; n < N; ++n) {
		k = Wong_Assignment[n];
		Num_chan[k]++;
		Gmean[k] *= (double)SNR[k][n];		
		Hmean[k] += 1.0f/(float)SNR[k][n]; 
	}
	gm0 = (float) pow(Gmean[0], 1.0/(double)Num_chan[0]);
	for (k = 1; k < user; k++) {
		gmk = (float) pow(Gmean[k], 1.0/(double)Num_chan[k]);
		a[k] = -(float) Num_chan[k]/ (float) Num_chan[0] * gm0 / gmk;
		b[k] = (float) Num_chan[0]/(float)SNR[k][Hmin[k]] * 
				(gmk * Hmean[k] / (float)Num_chan[k] - gm0 * Hmean[0] / (float)Num_chan[0]);
		ab[k] = a[k]*b[k];
		pow_num -= ab[k];
		pow_den -= a[k]; 
	}
	User_pow[0] = pow_num / pow_den;
	for (k = 1; k < user; k++) {
		User_pow[k] = ab[k] - User_pow[0]*a[k];
	}		
	//  Once every user has total power assigned, allocate power amongst subchannels
		
	//  First assign power to the subchannel with the worst SNR	
	for (k = 0; k < user; k++)	{
		ptemp = (User_pow[k] /*- Vk[k]*/)/(float)Num_chan[k];
		Data_rate[k] = log2f(1 + ptemp*(float)SNR[k][Hmin[k]]);	
		User_pow[k] = ptemp + 1.0f/(float)SNR[k][Hmin[k]];
	}

	for (n = 0; n < N; ++n) {
		k = Wong_Assignment[n];
		if (Hmin[k] != n) {
			ptemp = User_pow[k] - 1.0f/(float)SNR[k][n];
			Data_rate[k] +=  log2f(1 + ptemp*(float)SNR[k][n]);	
		}
	}
		

	free(User_pow); 
	free(Num_chan); 
	free(Gmean);
	free(Hmean);
	free(a);
	free(b);
	free(ab); 
	return error;
}

//  Sets Wk as defined as (11) in Shen's paper for user k
int Shen_Palloc(int user, SNRArray *SNR, int *Shen_Assignment, float *Data_rate, 
				 float *Proportionality, int *Hmin)
{
	int k;
	int n;
	int error = 0;
	float ptemp;
	float sum;							//  Number of iterations used in the Newton-Raphson method
	//float sum2;							//  Number of iterations used in the Newton-Raphson method
	float fx;							//  Solution of equation (17) in Shen's paper, with P1,tot guessed
	//float dfx;							//  Soltuion to the derivative of equation (17) in Shen's paper, with P1, tot guessed

	
	float *User_pow;	
	int *Num_chan;
	double *Wk;							//  K array of constants based on SNR ratios, Wk
	float *ck;							//  K array of constants used in total power allocation, ck
	float *dk;							//  K array of constants used in total power allocation, dk

	User_pow = (float *) malloc(user*sizeof(float));
	Num_chan = (int *) malloc(user*sizeof(int));
	Wk = (double *) malloc(user*sizeof(double));
	ck = (float *) malloc(user*sizeof(float));
	dk = (float *) malloc(user*sizeof(float));
	
	for (k = 0; k < user; k++)	{		
		Num_chan[k] = 0;
		Wk[k] = 1.0;
	}
	set_params(Shen_Assignment, SNR, user, Proportionality, Num_chan, Hmin, Wk, dk, ck);
//  Need to initialize some variables first
	/**/

//  First step: solve for user 1's total power
	User_pow[0] = tot_Power / (float)user;
	fx = 1;
	sum = 0;
	while ((fx > 0) || (fx < (0 - tot_Power*.00001)))
	{
		User_pow[0] = Newt_Raph(User_pow[0], user, ck, dk, &fx);
		sum++;
		if (sum == 50) break;
	}
	if (sum < 50) {
	//  Now solve for every other user's total power
		for (k = 1; k < user; k++)
		{
			User_pow[k]=ck[k] * powf(User_pow[0], dk[k]);
		}
		
	//  Once every user has total power assigned, allocate power amongst subchannels
		for (k = 0; k < user; k++)
		{

	//  First assign power to the subchannel with the worst SNR
			ptemp = (User_pow[k] /*- Vk[k]*/)/(float)Num_chan[k];
			Data_rate[k] = log2f(1 + ptemp*(float)SNR[k][Hmin[k]]);	
			User_pow[k] = ptemp + 1.0f/(float)SNR[k][Hmin[k]];
		}
	} else {
		error++;
		for (k = 0; k < user; k++) {
			User_pow[k] = tot_Power / (float)user;
		}
	}
	for (n = 0; n < N; ++n) {
		k = Shen_Assignment[n];
		if (Hmin[k] != n) {
			ptemp = User_pow[k] - 1.0f/(float)SNR[k][n];
			Data_rate[k] +=  log2f(1 + ptemp*(float)SNR[k][n]);	
		}
	}
		
	free(User_pow); 
	free(Num_chan); 
	free(Wk); 
	free(ck); 
	free(dk); 
	return error;
}

void set_params(int *Shen_Assignment, SNRArray *SNR, int user, float *Proportionality, 
		        int *Num_chan, int *Hmin, double *Wk, float *dk, float *ck)
{
	int n, k;
	
	
	for (n = 0; n < N; n++) {
		//if (Shen_Assignment[n] == k) { // if subcarrier i is assigned to user k
			k = Shen_Assignment[n];
			Num_chan[k]++;
			Wk[k] *= (double)((float)SNR[k][n] / (float) SNR[k][Hmin[k]]);
	}
	for (k = 0; k < user; ++k) {
		Wk[k] = pow(Wk[k],1.0/(double)Num_chan[k]);
		if (k == 0) {
			dk[k] = 1;
			ck[k] = 1;
		} 
		else {
			dk[k] = (Num_chan[0] * Proportionality[k]) / (Num_chan[k] * Proportionality[0]);
			ck[k] = ( (float)Num_chan[k] / ((float)SNR[k][Hmin[k]] * (float)Wk[k]) );
			ck[k] *= powf( (((float)SNR[k][Hmin[0]] * (float)Wk[0]) / ((float)Num_chan[0])), dk[k]);	
		}
	}
}


float Newt_Raph(float guess, int user, float *ck, float *dk, float *fx)
{
	int i;
	float temp = 0;
	float dfx = 0;

//  Calculate f(x) and df(x) at the same time
	for (i = 0; i < user; i++)
	{
		temp += ck[i]*powf((guess), dk[i]);
		dfx += (ck[i] * dk[i]) * powf((guess), (dk[i] - 1));	
	}
	*fx = temp - tot_Power;
	return guess - (*fx / dfx);
}