/*
 *  acm : an aerial combat simulator for X
 *  HSI tuner and HSI indicator module
 *  Copyright (C) 1991-1998  Riley Rainey
 *  Copyright (C) 2007  Umberto Salsi
 *
 *  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; version 2 dated June, 1991.
 *
 *  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., 675 Mass Ave., Cambridge, MA 02139, USA.
 */

#include <string.h>
#include <math.h>
#include "../util/memory.h"
#include "pm.h"
#include "navaid.h"
#include "../util/units.h"
#include "vpath_gallery.h"

#define hsi_IMPORT
#include "hsi.h"

/* how many NAV receivers */
#define HSI_NAV_MAX 2

/* how many RNAV calculators */
#define HSI_RNAV_MAX 5

/* repeat station reception check every ... (s) */
#define RECEPTION_CHECK_INTERVAL 4.0

/*
 *  Every station sends its Morse ID every ... (s)
 *  
 *  In the reality different stations can send their ID at different intervals
 *  of time.
 */
#define ID_DELAY 10.0

/*
 *  If we are lucky, the DME station will reply to our requests every so
 *  many seconds. This module simulates a DME receiver that performs some
 *  estimations in order to update the DME readout.
 *  
 *  In the reality, DMEs may or may not respond at all, and the responses
 *  come at random intervals of times.
 */
#define DME_DELAY 3.0

/*
 *  Every CDI dot means that angular deviation.
 */
#define CDI_DOT_STEP         units_DEGtoRAD(1.7)

/*
 *  Constant that maps the current angular deviation to some displacement
 *  of the CDI segment as drawn inside a circumference of unit radius.
 */
#define CDI_DOT_K            (0.9 / CDI_MAX_DEVIATION)

/*
 *  Number of dots to the left and to the right of the OBS indicator.
 */
#define CDI_DOT_NO           5

/*
 *  The CDI stops if the angular deviation is greater than that.  We allow
 *  the CDI segment to go half-dot beyond the last dot, just to alert the
 *  pilot that the indicator is out of scale.
 */
#define CDI_MAX_DEVIATION    ((CDI_DOT_NO + 0.5)*CDI_DOT_STEP)

/*
 *  The GS indicator stops at +/-2.5 DEG deviation
 */
#define GS_OFFSET_MAX        units_DEGtoRAD(2.5)

/*
 *  Constant that maps the current GS deviation to some displacement of the
 *  GS indicator.
 */
#define GS_OFFSET_K          (1.0/units_DEGtoRAD(2))


/*
 *  Way point: can be either a NAVAID station (VOR, DME, ILS) or a proper WP
 *  relative to some VOR/DME station for the RNAV calculator.
 */
typedef struct {
	navaid_Channel     frequency;  /* selected freq. */
	navaid_Type  *station;    /* NAVAID station, possibly NULL */
	double     station_at; /* last reception check */
	double     radial;     /* current radial occupied */
	double     id_update_at;  /* when to update the id field (s) */
	char      *id;            /* station name -- invalid if NULL */

	int        obs;        /* OBS (DEG, 0-359) */
	_BOOL      is_wp;      /* if it is a WP, the following fields have meaning: */

	/* RNAV data -- meaningful only if is_wp */
	int        rho;     /* distance VOR-WP (tenth of NM, 1-2000) */
	int        theta;   /* WP bearing relative to the VOR (DEG, 0-359) */
	double     cos_theta, sin_theta;

	/* Handling of the DME signal: */
	double     dme_response;     /* last response from DME, invalid if < 0 (NM) */
	double     dme_response_at;  /* last response received at this time (s) */
	double     dme_response_dot; /* time derivative of the responses (NM/s) */
	double     dme;              /* estimated distance, invalid if < 0 (NM) */

	/* Values to be displayed: */
	double     radial_readout;   /* current radial relative to the WP (RAD) */
	double     dme_readout;      /* distance from WP (NM) */
} waypoint;


typedef struct _hsi_data {
	struct _hsi_data *next;

	_BOOL enabled;
	int idx;  /* index of the current WP displayed */
	waypoint wps[HSI_NAV_MAX + HSI_RNAV_MAX];
} hsi_data;

/* Released HSI available for re-use. */
static hsi_data *free_list = NULL;

static vpath_Type
	* obs_pointer = NULL,
	* cdi_segment = NULL,
	* vor_orientation = NULL,
	* gs_dots = NULL,
	* gs_pointer = NULL;


static void hsi_cleanup()
{
	hsi_data *p;

	while( free_list != NULL ){
		p = free_list;
		free_list = free_list->next;
		memory_dispose(p);
	}

	memory_dispose(obs_pointer);  obs_pointer = NULL;
	memory_dispose(cdi_segment);  cdi_segment = NULL;
	memory_dispose(vor_orientation);  vor_orientation = NULL;
	memory_dispose(gs_dots);  gs_dots = NULL;
	memory_dispose(gs_pointer);  gs_pointer = NULL;
}


/* Set the "no DME readout" status */
static void hsi_reset_dme(waypoint *wp)
{
	wp->dme_response = -1.0; /* still no DME response */
	wp->dme_response_at = curTime;
	wp->dme_response_dot = 0.0;
	wp->dme = -1.0; /* still no DME estimation */
}


void hsi_enable(viewer *u)
{
	int i;
	hsi_data *hsi;
	waypoint *wp;

	if( u->hsi == NULL ){

		/* Allocate memory for HSI status: */
		if( free_list == NULL ){
			hsi = memory_allocate( sizeof(hsi_data), NULL );
			memory_registerCleanup(hsi_cleanup);
		} else {
			hsi = free_list;
			free_list = hsi->next;
		}

		/* Initialize: */
		hsi->idx = 0;
		for( i = 0; i < HSI_NAV_MAX + HSI_RNAV_MAX; i++ ){
			wp = &hsi->wps[i];
			wp->frequency = navaid_VOR_CHANNEL_MIN;
			wp->station = NULL;
			wp->station_at = 0.0;
			wp->radial = 0.0;
			wp->obs = 0;
			wp->is_wp = (i >= HSI_NAV_MAX);
			wp->rho = 10;  /* 1.0 NM */
			wp->theta = 0;
			wp->id = NULL;
			hsi_reset_dme(wp);
			wp->radial_readout = 0.0;
			wp->dme_readout = 0.0;
		}

		u->hsi = hsi;
	} else {
		hsi = u->hsi;
	}

	hsi->enabled = TRUE;
}


void hsi_disable(viewer *u)
{
	hsi_data *hsi;

	hsi = u->hsi;
	if( hsi == NULL )
		return;
	
	hsi->enabled = FALSE;
}


static void hsi_retune(waypoint *wp)
{
	/* Reset VOR receiver: */
	wp->station = NULL;
	wp->station_at = curTime;
	wp->radial = 0.0;
	wp->id = NULL;
	wp->radial_readout = 0.0;
	wp->dme_readout = 0.0;

	/* Reset DME receiver: */
	hsi_reset_dme(wp);
}




void hsi_update(viewer * u)
{
	navaid_Type *n;
	VPoint v;
	double d;
	
	hsi_data *hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	waypoint *wp = &hsi->wps[hsi->idx];

	/*
	 *  Determinate the station 'n', possibly re-scanning all the
	 *  receivable stations at this freq.
	 */
	if( curTime < wp->station_at + RECEPTION_CHECK_INTERVAL ){
		n = wp->station;
	
	} else {
		wp->station_at = curTime;

		n = navaid_reception_check(u->c, wp->frequency);

		if( n != NULL && wp->is_wp && (n->type & navaid_VOR) == 0 ){
			/* can't tune LOC in RNAV mode: */
			n = NULL;
		}
	}

	if( n == NULL ){
		/* no station found. */
		wp->station = NULL;
		wp->id = NULL;
		hsi_reset_dme(wp);

	} else if( wp->station != n ){
		/* Station change: */
		wp->station = n;
		wp->id_update_at = curTime + ID_DELAY;
		wp->id = NULL;
		wp->radial = 0.0;
		hsi_reset_dme(wp);
		wp->dme_response_at = curTime;

	} else {

		/* Still the same station. Update station ID: */
		if( wp->id == NULL && curTime > wp->id_update_at )
			wp->id = n->id;

		/* Update occupied radial: */
		VTransform(&u->c->Sg, &n->lt, &v);
		wp->radial = pm_heading(&v);
		if( ! wp->is_wp )
			wp->radial_readout = wp->radial;

		/* Update DME: */
		if( (n->type & navaid_DME) != 0
		&& curTime > wp->dme_response_at + DME_DELAY ){
			/* Got DME signal! */
			d = units_METERStoNM( VMagnitude(&v) );
			if( wp->dme_response >= 0.0 ){
				wp->dme_response_dot = (d - wp->dme_response)
					/ (curTime - wp->dme_response_at);
			}
			wp->dme_response = d;
			wp->dme_response_at = curTime;
			wp->dme = d;

		} else if( wp->dme_response >= 0.0 ){
			/*
				No response from DME but previous data available.
				Compute estimated value.
			*/
			wp->dme = fabs( wp->dme_response
				+ wp->dme_response_dot * (curTime - wp->dme_response_at) );
		}
	
		/*
		 *  Update values to be read by pilot and HSI display:
		 */
		if( wp->is_wp ){
			/*
				RNAV. Convert polar coords (dme,radial) relative
				to the VOR/DME to polar coords (dme_readout,radial_readout)
				relative to the WP:
			*/
			v.x = wp->dme * cos(wp->radial) - 0.1 * wp->rho * wp->cos_theta;
			v.y = wp->dme * sin(wp->radial) - 0.1 * wp->rho * wp->sin_theta;
			v.z = 0.0;
			wp->radial_readout = pm_heading(&v);
			wp->dme_readout = VMagnitude(&v);

		} else {
			/*
			 *  NAV. Simply copy better estimation of the
			 *  distance:
			 */
			wp->dme_readout = wp->dme;
		}
	}
}


void hsi_switch_mode(viewer * u)
{
	hsi_data *hsi;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	hsi->idx++;
	if( hsi->idx >= HSI_NAV_MAX + HSI_RNAV_MAX )
		hsi->idx = 0;
	
	hsi_retune( &hsi->wps[ hsi->idx ] );
}


static _BOOL is_auto_repeat( double *timeout )
{
	_BOOL res;

	res = (curTime < *timeout);
	*timeout = curTime + 0.1;
	return res;
}


void hsi_frq_inc(viewer * u, int step)
{
	hsi_data *hsi;
	waypoint *wp;
	static double timeout = 0.0;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	wp = &hsi->wps[hsi->idx];
	if( is_auto_repeat(&timeout) )
		step *= 2;
	wp->frequency += step;
	if( wp->frequency < navaid_VOR_CHANNEL_MIN )
		wp->frequency = navaid_VOR_CHANNEL_MIN;
	if( wp->frequency > navaid_VOR_CHANNEL_MAX )
		wp->frequency = navaid_VOR_CHANNEL_MAX;
	hsi_retune(wp);
}


void hsi_obs_inc(viewer * u, int step)
{
	hsi_data *hsi;
	waypoint *wp;
	static double timeout = 0.0;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	wp = &hsi->wps[hsi->idx];
	if( is_auto_repeat(&timeout) )
		step *= 2;
	wp->obs += step;
	while( wp->obs < 0 )
		wp->obs += 360;
	while( wp->obs >= 360 )
		wp->obs -= 360;
}


void hsi_theta_inc(viewer * u, int step)
{
	hsi_data *hsi;
	waypoint *wp;
	static double timeout = 0.0;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	wp = &hsi->wps[hsi->idx];
	if( ! wp->is_wp )
		return;
	if( is_auto_repeat(&timeout) )
		step *= 2;
	wp->theta += step;
	while( wp->theta < 0 )
		wp->theta += 360;
	while( wp->theta >= 360 )
		wp->theta -= 360;
	wp->cos_theta = cos( units_DEGtoRAD(wp->theta) );
	wp->sin_theta = sin( units_DEGtoRAD(wp->theta) );
	wp->dme_response_dot = 0.0;
}


void hsi_rho_inc(viewer * u, int step)
{
	hsi_data *hsi;
	waypoint *wp;
	static double timeout = 0.0;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	wp = &hsi->wps[hsi->idx];
	if( ! wp->is_wp )
		return;
	if( is_auto_repeat(&timeout) )
		step *= 5;
	wp->rho += step;
	if( wp->rho < 0 )
		wp->rho = 0;
	if( wp->rho > 2000 /* 200 NM */ )
		wp->rho = 2000;
	wp->dme_response_dot = 0.0;
}


int hsi_get_obs(viewer *u)
{
	hsi_data *hsi;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return 0;
	
	return hsi->wps[ hsi->idx ].obs;
}


_BOOL hsi_vor_radial(viewer *u, double *r)
{
	hsi_data *hsi;
	waypoint *wp;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return FALSE;

	wp = &hsi->wps[ hsi->idx ];

	if( wp->station == NULL
	|| (wp->station->type & navaid_VOR) == 0 )
		return FALSE;  /* no station tuned */
	
	if( wp->is_wp && wp->dme < 0.0 )
		return FALSE;  /* RNAV can't compute */

	*r = wp->radial_readout;
	return TRUE;
}


_BOOL hsi_dme(viewer *u, double *dist)
{
	hsi_data *hsi;
	waypoint *wp;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return FALSE;
	
	wp = &hsi->wps[ hsi->idx ];
	if( wp->station == NULL || wp->dme < 0.0 )
		return FALSE;
	
	*dist = wp->dme_readout;
	return TRUE;
}


_BOOL hsi_loc_radial(viewer *u, double *r)
{
	hsi_data *hsi;
	waypoint *wp;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return FALSE;

	wp = &hsi->wps[ hsi->idx ];

	if( wp->station == NULL
	|| (wp->station->type & navaid_LOC) == 0 )
		return FALSE;
	
	*r = wp->radial_readout;
	return TRUE;
}


_BOOL hsi_gs_offset(viewer *u, double *offset)
{
	hsi_data *hsi;
	waypoint *wp;
	VPoint    p;

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return FALSE;

	wp = &hsi->wps[ hsi->idx ];

	if( wp->station == NULL
	|| (wp->station->type & navaid_GS) == 0 )
		return FALSE;
	
	VTransform(&u->c->Sg, &wp->station->gst, &p);
	*offset = atan2(-p.z, p.x) - wp->station->slope;
	return TRUE;
}


static void hsi_panel_string(viewer *u, double x, double y, double fh,
	char *s1, char *s2, char *s3)
{
	double fw, fh2, fw2;

	fw = fh;

	fh2 = 1.5*fh;
	fw2 = fh2;

	VDrawStrokeString(u->v,
		(int) (x + 0.5), (int) (y + 0.5),
		s1, strlen(s1), (int) (fh + 0.5), radarColor);

	VDrawStrokeString(u->v,
		(int) (x + 13*fw - fw2*strlen(s2) + 0.5), (int) (y + 0.5),
		s2, strlen(s2), (int) (fh2 + 0.5), whiteColor);

	VDrawStrokeString(u->v,
		(int) (x + 14*fw + 0.5), (int) (y + 0.5),
		s3, strlen(s3), (int) (fh + 0.5), radarColor);
}


void hsi_panel_draw(viewer * u)
{
	hsi_data *hsi;
	waypoint *wp;
	Alib_Window *w;
	double x, y, fh, fw, il;
	int f;
	char s[100], r[100];

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	wp = &hsi->wps[hsi->idx];

	w = u->v->w;
	Alib_setClipRect(w, &u->tuner);
	Alib_fillRect(w, &u->tuner, panelBackgroundColor);

	fh = RectWidth(u->tuner) / 20.0;
	fw = fh;
	il = 1.2*fh*2.5; /* inter-line spacing */

	x = u->tuner.a.x + fw;
	y = u->tuner.a.y + il;

	/*
	 *  Displays mode:
	 */
	if( wp->is_wp )
		sprintf(s, "RNAV%d", hsi->idx - HSI_NAV_MAX + 1);
	else
		sprintf(s, "NAV%d", hsi->idx + 1);
	hsi_panel_string(u, x, y, fh, "Mode", s, "");
	y += il;

	/*
	 *  Displays frequency
	 */
	f = 10800 + 5 * (wp->frequency - navaid_VOR_CHANNEL_MIN);
	sprintf(s, "%d.%02d", f / 100, f % 100);
	hsi_panel_string(u, x, y, fh, "FRQ", s, "MHz");
	y += il;

	/*
	 *  Displays station ID:
	 */
	if( wp->id != NULL )
		strcpy(s, wp->station->id);
	else
		strcpy(s, "----");
	hsi_panel_string(u, x, y, fh, "STA", s, "");
	y += il;

	
	/*
	 *  Display DME:
	 */
	if( wp->dme >= 0.0 ){
		sprintf(s, "%0.1f", wp->dme_readout);
	} else {
		strcpy(s, "---.-");
	}
	hsi_panel_string(u, x, y, fh, "DME", s, "NM");
	y += il;

	/*
	 *  Displays WP setting:
	 */
	if( wp->is_wp ){
		sprintf(s, "%3d", wp->theta);
		sprintf(r, "%d.%d", wp->rho / 10, wp->rho % 10);
	} else {
		strcpy(s, "");
		strcpy(r, "");
	}
	hsi_panel_string(u, x, y, fh, "RAD", s, "DEG");
	y += il;
	hsi_panel_string(u, x, y, fh, "DST", r, "NM");
	y += il;
}


static void add_dot(vpath_Type *p, double ox, double oy, double r)
{
	VPoint a, b, c, d;

	VSetPoint(&a, ox + r, oy, 0.0);
	VSetPoint(&b, ox, oy + r, 0.0);
	VSetPoint(&c, ox - r, oy, 0.0);
	VSetPoint(&d, ox, oy - r, 0.0);

	vpath_moveTo(p, &a);
	vpath_lineTo(p, &b);
	vpath_lineTo(p, &c);
	vpath_lineTo(p, &d);
	vpath_lineTo(p, &a);
}


static void build_vpaths()
/*
 *  Build Vpats specific of the HSI. Every vpath is build over the xy plane in
 *  screen coords.: x=right, y=down. Vpaths that have a circular symmetry or
 *  need to be rotated, are build around the origin (0,0) and have a radius
 *  of 1.0 so that they can be easily rotated, scaled and translated to the
 *  final screen location.
 */
{
	int i;
	double l1, l2, l3, dev;

	if( obs_pointer != NULL )
		return;  /* all the vpats already done */
	
	/*
	 *  Build obs_pointer, including CDI scale
	 */

	l1 = 0.06;  /* OBS pointer half-width */
	l2 = 0.60;  /* CDI segment half-length */

	obs_pointer = vpath_new();
	vpath_moveTo(obs_pointer, &(VPoint){0.0, -1.00, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){ l1, -1.00+3.0*l1, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){ l1, -l2, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){ -l1, -l2, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){ -l1, -1.00+3.0*l1, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){0.0, -1.00, 0.0});

	vpath_moveTo(obs_pointer, &(VPoint){l1, l2, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){l1, 1.0, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){-l1, 1.0, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){-l1, l2, 0.0});
	vpath_lineTo(obs_pointer, &(VPoint){l1, l2, 0.0});

	for( i = 1; i <= CDI_DOT_NO; i++ ){
		dev = i * CDI_DOT_STEP * CDI_DOT_K;

		add_dot(obs_pointer, dev, 0.0, 0.03);
		add_dot(obs_pointer, -dev, 0.0, 0.03);
	}

	/*
	 *  Build CDI segment
	 */

	cdi_segment = vpath_new();
	vpath_moveTo(cdi_segment, &(VPoint){l1, -l2, 0.0});
	vpath_lineTo(cdi_segment, &(VPoint){l1, l2, 0.0});
	vpath_lineTo(cdi_segment, &(VPoint){-l1, l2, 0.0});
	vpath_lineTo(cdi_segment, &(VPoint){-l1, -l2, 0.0});
	vpath_lineTo(cdi_segment, &(VPoint){l1, -l2, 0.0});

	/*
	 *  Build VOR orientation indicator
	 */

	vor_orientation = vpath_new();
	vpath_moveTo(vor_orientation, &(VPoint){0.0, -0.50, 0.0});
	vpath_lineTo(vor_orientation, &(VPoint){2.0*l1, -0.50 + 2.0*l1, 0.0});
	vpath_lineTo(vor_orientation, &(VPoint){-2.0*l1, -0.50 + 2.0*l1, 0.0});
	vpath_lineTo(vor_orientation, &(VPoint){0.0, -0.50, 0.0});

	/*
	 *  Build GS scale
	 */

	gs_dots = vpath_new();
	add_dot(gs_dots, 0.0, 0.0, 0.05);
	add_dot(gs_dots, 0.0, 0.5, 0.03);
	add_dot(gs_dots, 0.0, 1.0, 0.03);
	add_dot(gs_dots, 0.0, -0.5, 0.03);
	add_dot(gs_dots, 0.0, -1.0, 0.03);

	/*
	 *  Build GS pointer
	 */

	l3 = 0.10;
	gs_pointer = vpath_new();
	vpath_moveTo(gs_pointer, &(VPoint){0.0, 0.0, 0.0});
	vpath_lineTo(gs_pointer, &(VPoint){-l3, 2.0*l3, 0.0});
	vpath_lineTo(gs_pointer, &(VPoint){-l3, -2.0*l3, 0.0});
	vpath_lineTo(gs_pointer, &(VPoint){0.0, 0.0, 0.0});
}


void hsi_draw(viewer * u)
{
	hsi_data *hsi;
	waypoint *wp;
	Alib_Window  *w;
	Alib_Pixel     white, magenta;
	int       xc, yc, h, x, y;
	int       vor_orient;
	VMatrix   m;
	double    scale, obs_scale, gs_scale, hdg, gs_offset, r, diff, dev;
	char      s[8];

	hsi = u->hsi;
	if( hsi == NULL || ! hsi->enabled )
		return;
	
	wp = &hsi->wps[hsi->idx];

	w = u->w;
	Alib_setClipRect(w, &u->indicator);
	Alib_fillRect(w, &u->indicator, panelBackgroundColor);

	build_vpaths();
	white = whiteColor;
	magenta = magentaColor;

	x = u->indicator.a.x;
	y = u->indicator.a.y;
	scale = RectWidth(u->indicator);
	r = 0.44*scale;
	obs_scale = 0.80 * r;
	gs_scale = 0.60 * r;

	xc = x + (int) (0.48*scale+0.5);
	yc = y + (int) (0.50*scale+0.5);

	if (u->c->showMag)
		hdg = pm_mag_heading(u->c);
	else
		hdg = u->c->curHeading;

	h = (int) (0.045*scale + 0.5);

	/*
	 *  Displays "TH" or "MH"
	 */
	VDrawStrokeString(u->v,
		x + (int) (0.05*scale+0.5), y + (int) (0.06*scale),
		(u->c->showMag)? "MH":"TH", 2, h, whiteColor);

	/*
	 *  Display the OBS value.
	 */
	VDrawStrokeString(u->v,
		x + (int) (0.85*scale), y + (int) (0.06*scale),
		"OBS", 3, h, whiteColor);
	sprintf(s, "%03d", wp->obs);
	VDrawStrokeString(u->v,
		x + (int) (0.85*scale), y + (int) (0.13*scale),
		s, 3, h, whiteColor);

	/* Draw compass scale: */
	VIdentMatrix(&m);
	VRotate(&m, ZRotation, -hdg);
	VScaleMatrix(&m, r, r, r);
	VTranslate(&m, xc, yc, -1.0);
	vpath_stroke( vpath_gallery_get_compass_scale(), &m, w, white);

	/* Draw compass fixed scale: */
	VIdentMatrix(&m);
	VScaleMatrix(&m, r/0.90, r/0.90, r/0.90);
	VTranslate(&m, xc, yc, -1.0);
	vpath_stroke( vpath_gallery_get_compass_fixed_scale(), &m, w, white);

	/* Draw stylized aircraft: */
	VIdentMatrix(&m);
	VScaleMatrix(&m, 0.25*r, 0.20*r, 0.25*r);
	VTranslate(&m, xc, yc, -1.0);
	vpath_stroke( vpath_gallery_get_stylized_aircraft(), &m, w, white);

	/* Draw OBS pointer with CDI scale: */
	VIdentMatrix(&m);
	VRotate(&m, ZRotation, units_DEGtoRAD(wp->obs) - hdg);
	VScaleMatrix(&m, obs_scale, obs_scale, obs_scale);
	VTranslate(&m, xc, yc, -1.0);
	vpath_stroke( obs_pointer, &m, w, white);


/*
 *  Draw the Course Deviation Indicator
 */

	if( wp->station != NULL
	&& wp->station->type & (navaid_VOR|navaid_LOC)
	&& (! wp->is_wp || wp->dme >= 0.0) ) {

/*
 *  Full localizer/SDF offsets will vary based on the beam width of the
 *  transmitter (typical localizer values are 3 to 6 degrees).
 *
 *  VOR offsets are 1.7 degrees per dot on the HSI.
 *  ILS course offsets are 0.4 degrees per dot.
 */

		diff = wp->radial_readout;

		if( wp->station->type & navaid_LOC ){
			/*
			 *  Calculate actual angular deviation from the
			 *  LOCATOR as angle between -M_PI and +M_PI:
			 */
			if( diff > M_PI )
				diff = diff - 2*M_PI;

			vor_orient = 0;
		}
		else {
			diff = diff - units_DEGtoRAD(wp->obs);

			/*
			 *  Calculate actual angular deviation from the
			 *  selected OBS as angle between -M_PI and +M_PI:
			 */
			if( diff > M_PI ){
				diff = diff - 2*M_PI;
			}
			else if( diff < -M_PI ){
				diff = 2*M_PI + diff;
			}

			if( fabs(diff) <= M_PI/2 ){
				/*
				 *  We are in the "FROM" half-plane:
				 */
				vor_orient = -1;
				diff = -diff;
			} else {
				/*
				 *  We are in the "TO" half-plane. Reduce
				 *  angular reviation relative to the
				 *  complementar radial OBS+M_PI:
				 */
				vor_orient = 1;
				if( diff > 0 )
					diff = diff - M_PI;
				else
					diff = M_PI + diff;
			}

		}

		/* Draw CDI segment: */
		VIdentMatrix(&m);

		/* Little hack for LOC CDI: increase sensibility from CDI_DOT_STEP
		   to 0.4 DEG/DOT: */
		if( wp->station->type & navaid_LOC ){
			diff *= CDI_DOT_STEP / units_DEGtoRAD(0.4);
		}

		if( diff > CDI_MAX_DEVIATION )
			dev = CDI_MAX_DEVIATION;
		else if( diff < -CDI_MAX_DEVIATION )
			dev = -CDI_MAX_DEVIATION;
		else
			dev = diff;

		VTranslate(&m, CDI_DOT_K * dev, 0.0, 0.0);
		VRotate(&m, ZRotation, units_DEGtoRAD(wp->obs) - hdg);
		VScaleMatrix(&m, obs_scale, obs_scale, obs_scale);
		VTranslate(&m, xc, yc, -1.0);
		vpath_stroke( cdi_segment, &m, w, magenta);

		/* Draw VOR TO/FROM indicator: */
		if( vor_orient != 0 ){

			VIdentMatrix(&m);

			if( vor_orient == 1 ){

				VRotate(&m, ZRotation, units_DEGtoRAD(wp->obs) - hdg);

			} else if( vor_orient == -1 ){

				VRotate(&m, ZRotation, units_DEGtoRAD(wp->obs) - hdg + M_PI);
			}

			VScaleMatrix(&m, obs_scale, obs_scale, obs_scale);
			VTranslate(&m, xc, yc, -1.0);
			vpath_stroke( vor_orientation, &m, w, white);
		}

	}

	/* Draw the Glide Slope scale: */
	VIdentMatrix(&m);
	VScaleMatrix(&m, gs_scale, gs_scale, gs_scale);
	VTranslate(&m, xc + 1.15*r, yc, -1.0);
	vpath_stroke(gs_dots, &m, w, white);

	/* Draw the Glide Slope Deviation Indicator: */
	if( wp->station != NULL && wp->station->type & navaid_GS ){

		if( ! hsi_gs_offset(u, &gs_offset) ) { }
		gs_offset = -gs_offset;

		if( gs_offset > GS_OFFSET_MAX )
			dev = GS_OFFSET_MAX;
		else if( gs_offset < -GS_OFFSET_MAX )
			dev = -GS_OFFSET_MAX;
		else
			dev = gs_offset;

		VIdentMatrix(&m);
		VTranslate(&m, 0.0, -dev*GS_OFFSET_K, 0.0);
		VScaleMatrix(&m, gs_scale, gs_scale, gs_scale);
		VTranslate(&m, xc + 1.10*r, yc, -1.0);
		vpath_stroke(gs_pointer, &m, w, magenta);
	}
}


void hsi_free(viewer *u)
{
	hsi_data *hsi;

	hsi = u->hsi;
	if( hsi == NULL )
		return;
	
	hsi->next = free_list;
	free_list = u->hsi;
	u->hsi = NULL;
}
