/****************************************************************
 * Thruster curves:
 *
 ****************************************************************/

#include "thruster_curves.h"

float lookupTable_foreaft[4096];
float lookupTable_turns[4096];
float lookupTable_lateral[4096];
float lookupTable_vertical[4096];

/**********************************************************************/


/* pilot deadband block */
float pilotDeadBand(float V_j, float VMAX, float DB)
{
	float scale_factor, V_jdb;

	scale_factor = VMAX/(VMAX-DB);
	V_jdb = (V_j - DB* sign(V_j)) * scale_factor;
	if (fabs(V_j) < DB)
		V_jdb = 0;
	return V_jdb;	
}


/* summation block */
float summation(float V_c, float V_jdb)
{
	return (V_c + V_jdb);
}


/* limter block (saturation) */
float limiter(float V_s, float VMAX)
{
	float V_l;

	V_l = V_s;
	if (fabs(V_s) > VMAX)
		V_l = VMAX * sign(V_s);
	return V_l;
}

/* pilot nonlinear mapping block */
float nonlinearMapping(float V_l, float VMAX, float ST, int Nlmode,
		       float KF1, float KF2, int whichThruster)
{
	float scale_factor = (VMAX-ST)/VMAX;
	float V_n;
	
	switch(Nlmode) {
		case OLDMODE:
			V_n = nlInverse(V_l, whichThruster);
			break;
		case PASSTHRU:
			V_n = V_l;
			break;
		case REVERSAL:
			V_n = nl(V_l, KF1, VMAX, KF2);
			break;
	}

	V_n = V_n * scale_factor + ST * sign(V_n);

	return V_n;

}



/************************** helper functions ********************/


float nl(float V_l, float KF1, float VMAX, float KF2)
{
	float VMAX_n, scale_factor, V_n;

	VMAX_n = sqrt(KF1 * pow(VMAX, 4) + KF2 * pow(VMAX, 2)) - 
		 sqrt(KF1) * pow(VMAX, 2);
	scale_factor = VMAX / VMAX_n;

	V_n = sqrt(KF1 * pow(V_l, 4) + KF2 * pow(V_l, 2)) - 
	      sqrt(KF1)*pow(V_l, 2);
	V_n *= scale_factor * sign(V_l);

	return V_n;

}


float int2float(int intVolt)
{
	return ((((float)intVolt)*10.00/ 4095.00) - 5.0);
}



int binsearch(float x, float v[], int n)
{
	/* This is a modified binary search in that it
	 * does not have to necessary find the exact
	 * match, but rather the closest
	 */	
	int low, high, mid;
        float high_diff, low_diff;
	
	low = 0;
	high = n - 1;

	while (low <= high) {
		mid = (low + high) / 2;
		if ( x < v[mid] )
			high = mid - 1;
		else if ( x > v[mid] )
			low = mid + 1;
		else
			return mid;
	}
	
	/* low and high now point to consecutive elements
	 * depending on which way the search
	 * ended
	 */
	if (low > 4095)
		return high;
	if (high < 0)
		return low;
	
	high_diff = fabs(x-v[high]);
	low_diff = fabs(x-v[low]);
	
	if (high_diff < low_diff)
		return high;
	return low;
	
	
}



float nlInverse(float V_l, int whichLookupTable)
{
	// assume we have access to a lookup table.
	// lookupTable is a float array with 4096 entries.
	// The lookup table need only be computed once when
	// the code is initialized
	int 	n;
	float 	V_n;	
	float*	lookupTablePtr = NULL;
	
	switch (whichLookupTable) {
		case L_FOREAFT:
			lookupTablePtr = lookupTable_foreaft; 
			break;
		case L_TURNS:
			lookupTablePtr = lookupTable_turns; 
			break;
		case L_LATERAL:
			lookupTablePtr = lookupTable_lateral; 
			break;
		case L_VERTICAL:
			lookupTablePtr = lookupTable_vertical; 
			break;
	}

	// searches for the value in lookup table
	n = binsearch(V_l, lookupTablePtr, 4096);	
	V_n = int2float(n);
       
	return (V_n);	
	
}



void initializeLookupTable(int whichLookupTable, float KF1, 
			   float VMAX, float KF2, float DB,
			   float ST)
{
	float V_temp;
	int n;

	//printf("whichLookupTable: %d\n", whichLookupTable);
	//printf("VMAX: %f\n", VMAX);
	//printf("KF1: %f\n", KF1);
	//printf("KF2: %f\n", KF2);
	//printf("DB: %f\n", DB);
	//printf("ST: %f\n", ST);
	
	for (n = 0; n < 4096; n++) {
		V_temp = int2float(n);

		switch (whichLookupTable) {
			case L_FOREAFT:
				lookupTable_foreaft[n] = 
					nl(V_temp, KF1, VMAX, KF2);
				break;
			case L_TURNS:
				lookupTable_turns[n] = 
					nl(V_temp, KF1, VMAX, KF2);
				break;
			case L_LATERAL:
				lookupTable_lateral[n] = 
					nl(V_temp, KF1, VMAX, KF2);
				break;
			case L_VERTICAL:
				lookupTable_vertical[n] = 
					nl(V_temp, KF1, VMAX, KF2);
				break;
		}
	
	}
	
	//for (n = 0; n < 4096; n++) {
	//	printf("%f\n", lookupTable_foreaft[n]);
	//}

}



float sign(float v)
{
	if (v < 0)
		return -1.00;
	return 1.00;
}