//////////////////////////////////////////////////////////////////////////////
/// compass.c
/// Compute north direction from magnetic/acceleration measures
///
/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
//////////////////////////////////////////////////////////////////////////////
// HISTORY
//  2012-12-01  [jDG] Creation
//  2012-12-23  [jDG] Added filtering.
//  2012-12-30  [jDG] Added calibration (spherical best fit).
//  2015-05-22  [jDG] Minor cleanups. Smaller calibration code.

#include "compass.h"

//////////////////////////////////////////////////////////////////////////////
// mH: crude work-around, needs to be fixed up
#ifndef UNIX
#	pragma udata overlay bank8=0x800
	static char C_STACK[256];			// overlay C-code data stack here
#	define RESET_C_STACK		\
		_asm					\
			LFSR	1, 0x800	\
			LFSR	2, 0x800	\
		_endasm
#	pragma udata overlay bank9_compass
#	pragma code compass_run
#else
#	define RESET_C_STACK
#endif

//////////////////////////////////////////////////////////////////////////////
// fifth order of polynomial approximation of atan(), giving 0.05 deg max error
//
#define K1	(5701)			// needs K1/2**16
#define K2	(1645)			// needs K2/2**48  WAS NEGATIV
#define K3	( 446)			// needs K3/2**80


//////////////////////////////////////////////////////////////////////////////
// Interface to assembler multiplies

Int16 umul(PARAMETER Int16 a, PARAMETER Int16 b)
{
	extern Int16 compass_umul(void);
	extern Int16 compass_a, compass_b;
	compass_a = a;
	compass_b = b;
	return compass_umul();
}

Int16 imul(PARAMETER Int16 a, PARAMETER Int16 b)
{
	extern Int16 compass_imul(void);
	extern Int16 compass_a, compass_b;
	compass_a = a;
	compass_b = b;
	return compass_imul();
}

//////////////////////////////////////////////////////////////////////////////
/// Returns a / b * 2**16
///
/// A 16/16 -> 16 bits divide, returning a scaled result.
/// Used to multiply fractional numbers in the range 0..1,
/// represented as 0..32767.

Int16 udiv(PARAMETER Int16 a, PARAMETER Int16 b)
{
	OVERLAY Int16 d, r;
    OVERLAY char failsafe=250;
    
	//---- Pre-scale both numerator and denominator --------------------------
	while( (((a>>8) | (b>>8)) & 0xC0) == 0 )
	{
        failsafe--;
        if (failsafe==0) break;  
		a <<= 1;
		b <<= 1;
	}

	//---- Make division trials ----------------------------------------------
	d = 0x4000;				// start with 0.5, because 1.0 is sign bit
	b >>= 1;				// hence pre-shift b
	r = 0;
	do {
		if( a >= b )		// a is big enough ?
		{
			a -= b;			// then count d times b out of it
			r |= d;			// and accumulate that bit
		}
		b >>= 1;			// then loop trying twice smaller
		d >>= 1;
	} while( b );
	return r;
}

//////////////////////////////////////////////////////////////////////////////
/// Computes atan(y/x) in Angle, for x, y in range 0..32767
///
/// Results a single quadrant Angle, in the range 0 .. Q_PI/2

Angle utan(PARAMETER Int16 y, PARAMETER Int16 x)
{
	OVERLAY Int16 ratio, angle, x2, x3;

	//---- Handle zero divisor -----------------------------------------------
	if( x == 0 )
		return (y == 0) ? 0 : Q_PIO2;

	//---- Make it half-quadrant : 0 .. 45 deg -------------------------------
	ratio = (x > y) ? udiv(y, x) : udiv(x, y);

	//---- Then apply the polynomial approximation ---------------------------
	angle = umul(K1, ratio);			// r*K1 / 2**16
	x2 = umul(ratio, ratio);			// r**2 / 2**16
	x3 = umul(x2, ratio);				// r**3 / 2**32
	angle -= umul(x3, K2);				// K2*r**3 / 2**48: NEGATIV.

	x3 = umul(x3, x2);					// r**5 / 2**64
	angle += umul(x3, K3);				// K3*r**5 / 2**80

	//---- Recover the full quadrant -----------------------------------------
	return (x < y) ? (Angle)(Q_PIO2 - angle)
				   : (Angle)(angle);
}

//////////////////////////////////////////////////////////////////////////////
/// Computes atan2(y/x) in Angle, for x, y in range -32768 to 32767
///
/// Results a four quadrant Angle, in the range -Q_PI .. +Q_PI

Angle itan(PARAMETER Int16 y, PARAMETER Int16 x)
{
	// Beware: -32768 is not properly handled (sign error)
	if( x == -32768 ) x = -32767;
	if( y == -32768 ) y = -32767;

	if( x >= 0 )
		if( y >= 0 )					// first  quadrant: 0..90 deg.
			return utan(y,x);
		else							// fourth quadrant: 0..-90 deg
			return -utan(-y,x);
	else
		if( y >= 0 )					// second quadrant: 90..180 deg
			return Q_PI - utan(y, -x);
		else							// third  quadrant: -90..-180 deg;
			return -Q_PI + utan(-y, -x);
}

//////////////////////////////////////////////////////////////////////////////
/// Computes cos(theta) = sqrtf(x2/h2),
/// when theta = atan(y/x) and h2=x*x+y*y
///

Int16 cosxh(PARAMETER Int16 x2, PARAMETER Int16 h2)
{
	OVERLAY Int16 r = 0;
	OVERLAY Int16 d = 0x4000;

	do {
		OVERLAY Int16 a = r + d;
		a = umul(a, a);
		a = umul(a, h2);
		if( a <= x2 ) r += d;
		d >>= 1;
	} while( d );

	return r;
}

//////////////////////////////////////////////////////////////////////////////
/// Computes both sin and cos of angle y/x,
/// with h = sqrt(x**2+y**2).
///

void sincos(PARAMETER Int16 x, PARAMETER Int16 y, Int16* sin, Int16* cos)
{
	OVERLAY Int16 x2, y2, h2;
    OVERLAY char failsafe=250;
    
	//---- Fold into one quadant ---------------------------------------------
	OVERLAY char neg = 0;
	if( x < 0 )
	{
		neg |= 1;
		x = -x;
	}
	if( y < 0 )
	{
		neg |= 2;
		y = -y;
	}
    
    //---- Pre-scale both numerator and denominator ----------------------
	while( (((x>>8) | (y>>8)) & 0xE0) == 0 )
	{
        failsafe--;
        if (failsafe==0) break;
		x <<= 1;
		y <<= 1;
	}

	//---- Uses trig() to do the stuff one on quadrant -------------------
	x2 = umul(x,x);
	y2 = umul(y,y);
	h2 = x2 + y2;
	x2 = cosxh(x2, h2);

	//---- Results back in four quadrants --------------------------------
	*cos = (neg & 1) ? -x2 : x2;
	y2   = cosxh(y2, h2);
	*sin = (neg & 2) ? -y2 : y2;
}

//////////////////////////////////////////////////////////////////////////////
//

void compass(void)
{
	OVERLAY Int16 sin, cos;
	OVERLAY Int16 iBfx, iBfy, Gz;
	OVERLAY Int16 iBpx, iBpy, iBpz;

	RESET_C_STACK;

	//---- Make hard iron correction -----------------------------------------
	// Measured magnetometer orientation, measured ok
	// From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation
	iBpx = compass_DX_f - compass_CX_f; // X
	iBpy = compass_DY_f - compass_CY_f; // Y
	iBpz = compass_DZ_f - compass_CZ_f; // Z

	//---- Calculate sine and cosine of roll angle Phi -----------------------
	sincos(accel_DZ_f, accel_DY_f, &sin, &cos);

	//---- rotate by roll angle (-Phi) ---------------------------------------
	iBfy = imul(iBpy, cos) - imul(iBpz, sin);
	iBpz = imul(iBpy, sin) + imul(iBpz, cos);
	Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);

	//---- calculate sin and cosine of pitch angle Theta ---------------------
	sincos(Gz, -accel_DX_f, &sin, &cos);	 // NOTE: changed sin sign

	/* correct cosine if pitch not in range -90 to 90 degrees */
	if( cos < 0 ) cos = -cos;

	///---- de-rotate by pitch angle Theta -----------------------------------
	iBfx = imul(iBpx, cos) + imul(iBpz, sin);

	//---- Detect uncalibrated compass ---------------------------------------
	if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
	{
		compass_heading = -1;
		return;
	}

	//---- calculate current yaw = e-compass angle Psi -----------------------
	// Result in degree (no need of 0.01 deg precision...
	compass_heading = itan(-iBfy, iBfx) / 100;

	// Result in 0..360 range:
	if( compass_heading < 0 )
		compass_heading += 360;
}