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

CSP - Control System Probe

Copyright (C) International Submarine Engineering Ltd. 1989


Module:		thrcurve.c

Description:	Fast conversion from sine and cosine to angle

Highlights:

History of modifications (4 most recent ones only):

%

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

#include	<comp\csp.h>
#ifdef __MSDOS__
#include	<math.h>
#endif

typedef struct thruster_component_stc	*Thruster;

/* -------------------- local procedures ------------------------------------ */

#ifdef __MSDOS__
Config_proc		install ();
Config_proc		generate_lookup_table (void);

Action_proc		enable (Thruster c, Boolean flag);
Action_proc		change_input (Thruster c, Float16 v);
static int		lookup_thruster_value (int);
#else
Config_proc		install ();
Config_proc		generate_lookup_table ();
Action_proc		enable ();
Action_proc		change_input ();
static int		lookup_thruster_value ();
#endif

/* -------------------- component definition -------------------------------- */

struct thruster_component_stc
{
	Component_header	header;
	Event			output;
	Active_event		input;
	Level			level;
	float			overlap;
	float			deadband;
	int			overlap16;
	int			deadband16;
	long			gain16;
};

static struct attribute_stc attributes[] =
{
	"output",		&event_attribute,
	"input",		&event_attribute,
	"level",		&level_attribute,
	"overlap",		&float_attribute,
	"deadband",		&float_attribute,
	NULL
};

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

static unsigned	*output_table, *slope_table;

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

#ifdef __MSDOS__
Config_proc  install (Thruster c)
#else
Config_proc  install (c)  Thruster c;
#endif
	{
	if (c->input.e->class == float16_data_class &&
		c->output->class == float16_data_class &&
		generate_lookup_table () &&
		(c->input.a = create_action (c->input.e,
			c->level, change_input, c)) != NULL)
		{
		c->overlap16 = float_to_float16 (c->output,
			c->overlap);
		c->deadband16 = float_to_float16 (c->input.a->event,
			c->deadband);
		if (c->deadband16 >  0)
			{
			c->gain16 = ((long) c->overlap16 << 16) /
				c->deadband16;
			c->overlap16 -= lookup_thruster_value (
				c->deadband16);
			}
		else
			{
			c->gain16 = 0;
			}
		return (TRUE);
		}
	else
		{
		return (FALSE);
		}
	}

/* -------------------- component action ------------------------------------ */

#ifdef __MSDOS__
Action_proc	enable (Thruster c, Boolean enabled)
#else
Action_proc	enable (c, enabled)  Thruster c; Boolean enabled;
#endif
	{
	set_enable (c->input.a, enabled);
	}

/* -------------------- component action ------------------------------------ */

#ifdef __MSDOS__
Action_proc	change_input (Thruster c, Float16 input)
#else
Action_proc	change_input (c, input)  Thruster c; Float16 input;
#endif
	{
	if (input > 0)
		{
		if (input < c->deadband16)
			{
			signal_event (c->output, (int)
				((c->gain16 * input) >> 16));
			}
		else
			{
			signal_event (c->output, lookup_thruster_value
				(input) + c->overlap16);
			}
		}
	else if (input < 0)
		{
		if (input > -c->deadband16)
			{
			signal_event (c->output, (int)
				((c->gain16 * input) >> 16));
			}
		else
			{
			signal_event (c->output, -lookup_thruster_value
				(-input) - c->overlap16);
			}
		}
	else
		{
		signal_event (c->output, 0);
		}
	}

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

#ifdef __MSDOS__
static int	lookup_thruster_value (int input)
#else
static int	lookup_thruster_value (input)  int input;
#endif
	{
	union {unsigned i; unsigned char b[2];} v;

	/* slope_table[] is equivalent to slope * 2^14;
	   lsb is equivalent to input error * 2^14;
	   output value is equivalent to result * 2^14 */

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

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

Config_proc	generate_lookup_table ()
	{
	int	index;
	float	output_value, prev_value;

	/* each table has 65 entries */
	if (output_table != NULL)
		{
		return (TRUE);
		}
	else if ((output_table = malloc (65 * sizeof (int))) != NULL &&
		 (slope_table = malloc (65 * sizeof (int))) != NULL)
		{
		output_table[0] = slope_table[0] = 0;
		prev_value = 0;
		for (index = 1; index <= 64; index++)
			{
			output_value = (float) (index * index) / (64.0 * 64.0);
			output_table[index] = (unsigned) (output_value * MAX_FLOAT16_VALUE);

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

			slope_table[index-1] = (unsigned) (
				(output_value - prev_value) * 0x100000l);
			prev_value = output_value;
			}
		return (TRUE);
		}
	else
		{
		return (FALSE);
		}
	}