/*
 *  acm : an aerial combat simulator for X
 *  Magnetic compass module
 *  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 "vpath.h"

#define magnetic_compass_IMPORT
#include "magnetic_compass.h"

typedef struct _magnetic_compass_data {
	double last_time;  /* last update */
	double hdg;        /* current indicated value (RAD) */
	double hdg_dot;    /* current rotation speed (RAD/s) */
	double x_rot, y_rot;  /* compass roll and pitch angles in aircraft frame */
} magnetic_compass_data;


static vpath_Type * compass_scale_path = NULL;


static void magnetic_compass_cleanup()
{
	memory_dispose(compass_scale_path);
	compass_scale_path = NULL;
}


void magnetic_compass_enable(viewer *u)
{
	magnetic_compass_data *mc;

	if( u->magnetic_compass == NULL ){
		mc = memory_allocate(sizeof(magnetic_compass_data), NULL);
		mc->last_time = curTime;
		mc->hdg = 0.0;
		mc->hdg_dot = 0.0;
		mc->x_rot = 0.0;
		mc->y_rot = 0.0;

		u->magnetic_compass = mc;
	} else {
		mc = u->magnetic_compass;
	}
}


void magnetic_compass_disable(viewer *u)
{
	magnetic_compass_free(u);
}


#define TILT_ANGLE units_DEGtoRAD(18)

void magnetic_compass_update(viewer *u)
{
	magnetic_compass_data *mc;
	double x_rot, y_rot, MH, dt;
	_BOOL tilted;
	VPoint aH, cH;
	VMatrix R;

	mc = u->magnetic_compass;
	if( mc == NULL )
		return;
	
	if( ! u->c->showMag ){
		// Magnetic field components not available.
		mc->last_time = curTime; // avoid abrupt turn if c->showMag enabled again
		return;
	}

	dt = curTime - mc->last_time + 0.01 /* div by zero workaround! */;
	mc->last_time = curTime;

	/*
		Update compass roll and pitch angles based on the local vertical
		u->c->G:
	*/
	tilted = FALSE;

	x_rot =  atan2(u->c->G.y, -u->c->G.z);
	y_rot = -atan2(u->c->G.x, -u->c->G.z);

	mc->x_rot += (x_rot - mc->x_rot) * 0.002 / dt;
	if( mc->x_rot < -TILT_ANGLE ){
		mc->x_rot = -TILT_ANGLE;
		tilted = TRUE;
	}
	if( mc->x_rot > TILT_ANGLE ){
		mc->x_rot = TILT_ANGLE;
		tilted = TRUE;
	}

	mc->y_rot += (y_rot - mc->y_rot) * 0.002 / dt;
	if( mc->y_rot < -TILT_ANGLE ){
		mc->y_rot = -TILT_ANGLE;
		tilted = TRUE;
	}
	if( y_rot > TILT_ANGLE ){
		y_rot = TILT_ANGLE;
		tilted = TRUE;
	}

	if( tilted )
		mc->hdg_dot = 0.0;

	/* Local magnetic field aH in aircraft frame (nT): */
	VReverseTransform_(&u->c->actualMagneticField, &u->c->trihedral, &aH);

	/* Local magnetic field cH in compass frame (nT): */
	VIdentMatrix(&R);
	VRotate(&R, YRotation, mc->y_rot);
	VRotate(&R, XRotation, mc->x_rot);
	VReverseTransform_(&aH, &R, &cH);

	/*
		MH is the magnetic heading in the compass frame, i.e. projection
		if cH on the local horizontal plane x-y of the aircraft:
	*/
	if( fabs(cH.x) + fabs(cH.y) < 1e-5 )
		return;
	MH = atan2(-cH.y, cH.x);
	if( MH < 0.0 )
		MH += 2*M_PI;

	/* Update rotational speed: */
	mc->hdg_dot +=

		/* Apply torque due to the magnetic field. 42000 nT is the max intensity
		 * of the mag. field on the Earth; 0.1 is a quite arbitrary factor
		 * accounting for the magnetic dipole of the compass. */
		0.1/42000 * dt * sin( MH - mc->hdg ) * sqrt(cH.x*cH.x + cH.y*cH.y)

		/* Apply viscosity (quite arbitrary coeff.). */
		- 0.007 * mc->hdg_dot;

	mc->hdg += mc->hdg_dot * dt;

	while( mc->hdg < 0.0 )
		mc->hdg += 2*M_PI;
	while( mc->hdg >= 2*M_PI )
		mc->hdg -= 2*M_PI;
}


/**
 * Return the scale of the compass, drawn on a cone of radius 1.0
 * and height 1.0. The axis of the cone is y and the pivot point is
 * the origin.
 */
static vpath_Type * get_compass_scale_path()
{
	vpath_Type *path;
	int a, s_len;
	double r, notch_short, notch_long, notch_len;
	double fh, fw, slant, margin;
	VPoint p, q;
	VMatrix M, L;
	char *s;
	static char *labels[] = {"N", "3", "6", "E", "12", "15", "S", "21",
		"24", "W", "30", "33"};

	path = vpath_new();

	r = 1.0;  /* radius and cone height */
	notch_short = 0.12 * r;
	notch_long = 0.25 * r;
	slant = units_DEGtoRAD(45);
	fh = 0.20 * r;  /* font height */
	fw = 0.12 * r;  /* font width */
	margin = 0.03 * r;  /* long notch to char margin */

	for( a = 0; a < 360; a += 5 ){

		/* Transformation for this notch + label */
		VIdentMatrix(&M);
		VRotate(&M, XRotation, -slant);
		VTranslate(&M, 0.0, r, -r);
		VRotate(&M, YRotation, -units_DEGtoRAD(a));

		/* Draw notch */
		if( a % 10 == 0 )
			notch_len = notch_long;
		else
			notch_len = notch_short;
		
		VSetPoint(&p, 0.0, 0.0, 0.0);
		VSetPoint(&q, 0.0, -notch_len, 0.0);
		VTransform(&p, &M, &p);
		VTransform(&q, &M, &q);
		vpath_moveTo(path, &p);
		vpath_lineTo(path, &q);

		/* Draw label */
		if( a % 30 == 0 ){

			s = labels[a/30];
			s_len = strlen(s);

			VIdentMatrix(&L);
			VScaleMatrix(&L, fw, fh, 1.0);
			VTranslate(&L, -0.5 * (fw * s_len), -notch_len - margin, 0.0);
			VMatrixMult(&L, &M, &L);

			vpath_draw_string(path, s, s_len, &L);
		}

	}

	return path;
}


void magnetic_compass_draw(viewer * u)
{
	magnetic_compass_data *mc;
	Alib_Window  *w;
	double    x, y, width, h, r, xmargin, ymargin;
	VMatrix   M;
	Alib_Rect      rect;
	
	if( ! u->c->showMag ){
		// Components of the magnetic field not available.
		return;
	}

	mc = u->magnetic_compass;
	if( mc == NULL )
		return;

	if( u->viewDirection.x < 0.90 )
		return;
	
	w = u->v->w;
	x = 0.85 * gui_getWidth(u->gui);
	y = 0.0;
	width = gui_getWidth(u->gui) - x;
	h = 0.50 * width;
	r = 0.45 * width; /* actual radius of the scale cone */

	Alib_setRect(&rect, x, 0, gui_getWidth(u->gui), h);
	Alib_setClipRect(w, &rect);
	Alib_fillRect(w, &rect, panelBackgroundColor);
	
	VIdentMatrix(&M);

	/* Set scale size */
	VScaleMatrix(&M, r, r, r);

	/* Rotate based on the current magnetic compass state */
	VRotate(&M, YRotation, mc->hdg);
	VRotate(&M, XRotation, mc->y_rot);
	VRotate(&M, ZRotation, mc->x_rot);

	/* Move to the final screen location */
	VTranslate(&M, x + width/2, y - r + h*2/3, 0.0);

	/* Draw compass scale */
	xmargin = 0.20 * width;
	ymargin = 0.10 * h;
	Alib_setRect(&rect, x + xmargin, y + ymargin, x + width - xmargin, y + h - ymargin);
	Alib_setClipRect(w, &rect);
	if( compass_scale_path == NULL ){
		compass_scale_path = get_compass_scale_path();
		memory_registerCleanup(magnetic_compass_cleanup);
	}
	vpath_stroke(compass_scale_path, &M, w, whiteColor);

	/* Draw reference line */
	Alib_drawLine(w, x + 0.5*width, y + 0.10*h, x + 0.5*width, y + 0.90*h,
		magentaColor);
}


void magnetic_compass_free(viewer *u)
{
	memory_dispose(u->magnetic_compass);
	u->magnetic_compass = NULL;
}
