// Calculates the update factor for linear least squares optimization

#include "lsq.h"

extern char errstr[200];  // error string that all routines have access to
extern int firstIterFlag;

int calc_update_factor(GRID_ARRAY *beamlets, GRID *dose, PRESC *presc, int beamlet_num, float *update_factor, PRESCGRID *prescGrid)
// Calculates a weight update factor based on the method described by 
// Shepard PMB 45, 69-90 (2000).  The weight update factor is based on the
// derivative of the objective function, which is zero when the minimum has
// been reached.
{
	int i,j,k,m;
	float Eplus_num, Eplus_den;     // overdose penalty terms
 	float Eminus_num, Eminus_den;   // underdose penalty terms
	float Edvh_plus_num, Edvh_plus_den;    // DVH overdose penalty terms
	float Edvh_minus_num, Edvh_minus_den;  // DVH underdose penalty terms
	float numerator, denominator;

	float A;
	float diff, doseValue, dPlus, dMinus, Dij;

	float del_d_plus;
	float del_d_minus;
	
	// initialize numerator and denominator of the update factor to zero
	numerator = 0.0;
	denominator = 0.0;

	for (j=0;j<beamlets->array[beamlet_num].size;j++)
	{
		i = beamlets->array[beamlet_num].indices[j];
		Dij = beamlets->array[beamlet_num].data[j];
		doseValue = dose->matrix[i];

		// initialize objective function terms for voxel i to zero
		Eplus_num = 0.0;
		Eplus_den = 0.0;
		Eminus_num = 0.0;
		Eminus_den = 0.0;
		Edvh_plus_num = 0.0;
		Edvh_plus_den = 0.0;
		Edvh_minus_num = 0.0;
		Edvh_minus_den = 0.0;

		for (m=0;m<prescGrid->numNodes[i];m++)
		{
			k = prescGrid->array[i][m].tissueInd;  // tissue type index
			A = presc->array[k].alpha/(float)presc->array[k].Nind;

			// overdose penalty term
			if (presc->array[k].betaPlus > 0.0)
			{
				dPlus = prescGrid->array[i][m].dPlus;
				if (doseValue > dPlus)
				{
					Eplus_num += presc->array[k].betaPlus*dPlus*A;
					Eplus_den += presc->array[k].betaPlus*doseValue*A;
				}
			}

			// underdose penalty term
			if (presc->array[k].betaMinus > 0.0)
			{
				dMinus = prescGrid->array[i][m].dMinus;
				if (doseValue < dMinus)
				{
					Eminus_num += presc->array[k].betaMinus*dMinus*A;
					Eminus_den += presc->array[k].betaMinus*doseValue*A;
				}
			}

			// DVH overdose penalty term
			if (presc->array[k].betaVPlus > 0.0)
			{
				del_d_plus = presc->array[k].dvh.del_d_plus;
				diff = doseValue - presc->array[k].dVPlus;
				if (diff >= 0.0 && diff <= del_d_plus)
				{
					Edvh_plus_num += presc->array[k].betaVPlus*presc->array[k].dVPlus*A;
					Edvh_plus_den += presc->array[k].betaVPlus*doseValue*A;
				}
			}

			// DVH underdose penalty term
			if (presc->array[k].betaVMinus > 0.0)
			{
				del_d_minus = presc->array[k].dvh.del_d_minus;
				diff = doseValue - presc->array[k].dVMinus;
				if (diff >= del_d_minus && diff <= 0.0)
				{
					Edvh_minus_num += presc->array[k].betaVMinus*presc->array[k].dVMinus*A;
					Edvh_minus_den += presc->array[k].betaVMinus*doseValue*A;
				}
			}
		}

		numerator += Dij*(Eplus_num + Eminus_num + Edvh_plus_num + Edvh_minus_num);
		denominator += Dij*(Eplus_den + Eminus_den + Edvh_plus_den + Edvh_minus_den);
	}

	// calculate the update factor, avoiding divide-by-zero errors
	if (denominator == 0.0)
		*update_factor = 0.0;   // diverging update factor should be forced to zero, turning that weight off
	else
		*update_factor = numerator/denominator;

	return(SUCCESS);
}