/*
--------------------------------------------------------------------------------

CSP - Control System Probe

Copyright (C) International Submarine Engineering Ltd. 1989


Module:		thrcurve.c

Description:	Thruster curve mapping to linearize the thrust-voltage
		characteristics of the hydraulic thrusters.
		The lookup table uses 64+1 points and there are hard coded
		conditions which are based on this assumption.

Highlights:

History of modifications (4 most recent ones only):

%Jan 25, 1996 - RMessier.

     1. Fixed out-of-array error--added an extra bin to 'output_table_plus[]'
	and 'slope_table_plus[]' arrays.
     2. Fixed discontinuity just at edge of linear_region.
     3. Flat areas of the output, and sudden steep areas of the output were
	removed--the ERROR_BOUND in the inverse function was changed from 6.0
	to 1x10E-6 of the desired thrust.
     4. Made the code easier to understand by renaming functions, adding some
	comments, and consolidating the thruster function's 'sqrt(k1)' out of
	the 'install()' routine and into the 'calc_thrust()' routine.

     5. The original thrust equation was:

			   ---           -----------------
                          /      2      /     4         2
	     Thrust = - \/ Kf1  V  +  \/ Kf1 V   + Kf2 V	(See *Note*)


	The new Eric/Dave/Luca derivation is:

				 2
			    Kf1 V

	     Thrust  =   ------------				(See *Note*)
			      2
		         Kf2 V  + Kf3

	*Note*: To use this new formula, configure 'kf3'.
		To use the original formula, configure 'kf3=0.0' (or leave it
		out of the configuration file).

     6.	Removed kf1_minus and kf2_minus from configuration. Made const
	attributes consistent by making configuration use 'kf3_plus'.

%July 8, 1991 - Glen re-wrote the component to use a function which better
	matches the characteristics than the quadratic curve used previously.
%

--------------------------------------------------------------------------------
*/
/*
// $Id: NEWCURVE.C 8.5 1996/02/20 20:05:17 filipozi Exp $
//
// Revisions:
//
// $Log: NEWCURVE.C $
// Revision 8.5  1996/02/20 20:05:17  filipozi
// Fixed minor bug. error_exit not available on the PC, while panic is.
//
// Revision 8.4  1996/02/13 14:30:27  filipozi
// Very minor mod to comment block.
//
// Revision 8.3  1996/02/13 14:18:13  filipozi
// Modifications made by rmessier. See comment above.
//
// Revision 8.2  1995/02/17 02:43:43  Dave
// Initial RCS version.
//
*/

#include	<comp\csp.h>
#include	<math.h>

typedef	struct thruster_component_stc
{
	Component_header	header;
	Event			output;
	Active_event		input;
	Level			level;
	float			forward_deadband;
	float			linear_region;
	float			kf1_plus;
	float			kf2_plus;
	float			kf3_plus;

	int			forward_deadband16;
	int			linear_region16;
	long			gain16;

	unsigned		output_table_plus[64+1];
	unsigned		slope_table_plus[64+1];

} *Thruster;

static struct attribute_stc attributes[] =
{
	"output",		&event_attribute,
	"input",		&event_attribute,
	"level",		&level_attribute,
	"forward_deadband",	&float_attribute,
	"linear_region",	&float_attribute,
	"kf1",			&float_attribute,
	"kf2",			&float_attribute,
	"kf3",			&float_attribute,
	NULL
};

/* --------------------------------------------------------------------- */

Action_proc enable (c, enabled)  Thruster c; Boolean enabled;
{
	set_enable (c->input.a, enabled);
}

#ifdef	__MSDOS__
static	int get_table_value (int input, unsigned *output_table,
			unsigned *slope_table)
#else
static	int get_table_value (input, output_table, slope_table)
	int input; unsigned *output_table, *slope_table;
#endif
{
	/* Return the appropriate value from the lookup tables.
	** (The next three lines are original comments.)
	**
	**	slope_table[] is equivalent to slope * 2^11;
	**	lsb is equivalent to input error * 2^11;
	**	output value is equivalent to result * 2^11
	*/

	union {unsigned i; unsigned char b[2];} v;

	v.i = input;

	#ifdef __MSDOS__
	return (output_table[v.b[1]] +
		(int) (((long) slope_table[v.b[1]] * v.b[0]) >> 11));
	#else
	return (output_table[v.b[0]] +
		(int) (((long) slope_table[v.b[0]] * v.b[1]) >> 11));
	#endif
}

Action_proc input_proc (c, input)  Thruster c; Float16 input;
{
	/* Use the tables defined at install time to output the inverse
	** thruster curve. This output drives the thruster and therefore
	** gives the operator linear control.
	*/

	if (input > -c->linear_region16  &&  input < c->linear_region16)
	{
		signal_event (c->output, (int) ((c->gain16 * input) >> 16));
	}
	else if (input > 0)
	{
		signal_event (c->output, get_table_value (input,
			c->output_table_plus, c->slope_table_plus) +
			c->forward_deadband16);
	}
	else /* if (input < 0) */
	{
		signal_event (c->output, -get_table_value (-input,
			c->output_table_plus, c->slope_table_plus) -
			c->forward_deadband16);
	}
}

/* --------------------------------------------------------------------- */

#ifdef	__MSDOS__
float	calc_thrust (float voltage, float kf1, float kf2, float kf3, float shift)
#else
float	calc_thrust (voltage, kf1, kf2, kf3, shift) float voltage, kf1, kf2, kf3, shift;
#endif
{
	/* Calculate the forward thrust.
	** (Input is "voltage", return value is "thrust").
	*/

	float	v, temp, value;

	v = voltage - shift;
	if (v < 0.0)
	{
#ifdef __MSDOS__
		panic ("Error--Negative voltage 'v' in NEWCURVE.C");
#else
		error_exit ( "Error -- Negative voltage in NEWCURVE.C" );
#endif __MSDOS__
	}

	/* IF 'kf3' is zero THEN use the original formula.
	** ELSE use the new formula.
	*/
	if (kf3 == 0.0)
	{
		temp = sqrt((double)kf1) * v*v;
		value = -temp + sqrt((double) (temp*temp + kf2*v*v));
	}
	else
	{
		value = (kf1 * v*v) / (kf2 * v*v + kf3);
	}

	return	value;
}

#ifdef	__MSDOS__
float	calc_inverse_thrust (float thrust, float kf1, float kf2, float kf3, float max_voltage)
#else
float	calc_inverse_thrust (thrust, kf1, kf2, kf3, max_voltage)
	float thrust, kf1, kf2, kf3, max_voltage;
#endif
{
	/* Calculate the inverse thrust.
	** (Input is "thrust", return value is "voltage").
	**
	** Implementation: Iteratively call the forward thrust function,
	** 'calc_trust(voltage)' until our result is within 'ERROR_BOUND'
	** difference of 'thrust'.
	*/

	float	voltage, error;
	float	interval;
	int	done;

	float	ERROR_BOUND = thrust / 1E6;

	voltage = max_voltage * 0.5;
	interval = voltage * 0.5;
	done = 0;
	while (!done)
	{
		error = calc_thrust (voltage, kf1, kf2, kf3, 0.0) - thrust;
		if (error > -ERROR_BOUND  &&  error < ERROR_BOUND)
		{
			done = 1;
		}
		else if (error > 0.0)
		{
			voltage = voltage - interval;
		}
		else
		{
			voltage = voltage + interval;
		}
		interval = interval*0.5;
	}
	return	voltage;
}

/* -------------------- component configuration subroutine ---------------- */

#ifdef	__MSDOS__
Config_proc generate_lookup_table (Thruster c)
#else
Config_proc generate_lookup_table (c) Thruster c;
#endif
{
	/* This routine prepares the tables 'output_table_plus[]', and
	** 'slope_table_plus[]' for use at runtime. They contain the inverse
	** trust values. The table is indexed from 0 to 64. (The high byte of
	** a positive 'float16' goes from 0 to 64).
	*/

	int	index;
	float	output_plus,  prev_plus;
	float	max_value;
	float	interval, range, desired_thrust;

	range = float16_event_range(c->input.a->event);
	max_value = calc_thrust (range, c->kf1_plus, c->kf2_plus, c->kf3_plus,
		0.0);
	interval = max_value / 64.0;

	c->output_table_plus[0] = 0;
	c->slope_table_plus[0]  = 0;
	c->slope_table_plus[64] = 0;
	prev_plus = 0;
	for (index = 1; index < 64+1; index++)
	{
		desired_thrust = index * interval;

		output_plus = calc_inverse_thrust (desired_thrust,
			c->kf1_plus, c->kf2_plus, c->kf3_plus, range);

		/* Normalize the output to the absolute maximum output. */
		output_plus = output_plus / (range + c->forward_deadband);

		c->output_table_plus[index] =
				(unsigned) (output_plus * MAX_FLOAT16_VALUE);

		/* slope = difference * 64, then shift left by 11 more
		   bits to a total of 17 bits */

		c->slope_table_plus[index-1]  = (unsigned) (
				(output_plus - prev_plus) * 0x20000l);

		prev_plus = output_plus;
	}

	return	TRUE;
}

/* -------------------- component configuration --------------------------- */

Config_proc install (c)  Thruster c;
{
	if (c->input.e->class != float16_data_class ||
		c->output->class != float16_data_class)
	{
		return	FALSE;
	}

	c->input.a = create_action (c->input.e, c->level, input_proc, c);

	generate_lookup_table(c);

	c->forward_deadband16 = float_to_float16 (c->output, c->forward_deadband);
	c->linear_region16 = float_to_float16 (c->input.a->event, c->linear_region);

	/* Define 'gain16'. */
	if (c->linear_region16 < 0)
	{
		return	FALSE;
	}
	if (c->linear_region16 > 0)
	{
		Float16 rise16 = c->forward_deadband16 +
			get_table_value (c->linear_region16,
			c->output_table_plus, c->slope_table_plus);
		c->gain16 = ((long) rise16 << 16) / c->linear_region16;
	}
	else
	{
		c->gain16 = 0;
	}

	return	TRUE;
}

struct template_stc new_thruster_curve_template =
{
	NULL, NULL, install, NULL, enable, "newthruster",
	attributes, NULL, sizeof (struct thruster_component_stc)
};