//
//  CompassBearing.m
//  CocoaSpatial
//
//  Created by Patrick McNeil on 10-05-21.
//  Copyright 2010 __MyCompanyName__. All rights reserved.
//

#import "CompassBearing.h"
#import "math.h"

void normalize(double vect[4][2])
{
	double length = sqrt(vect[1][1]*vect[1][1]+vect[2][1]*vect[2][1]+vect[3][1]*vect[3][1]);
	vect[1][1]/=length;
	vect[2][1]/=length;
	vect[3][1]/=length;
}

//C will be rows1 x cols2
void crossProduct(double *A, double *B, double *C, int rows1, int cols1, int rows2, int cols2)
{
	int i,j,k;
	
    if (cols1 != rows2)
	{
        printf("Inner matrix dimensions must agree.");
		return;
	}
	
    for (i = 1; i <= rows1; i++)
    {
        for (j = 1; j <= cols2; j++)
        {
			*(C + i*(cols2+1) + j) = 0;
            for (k = 1; k <= cols1; k++)
            {
				*(C + i*(cols2+1) + j) += *(A + i*(cols1+1) + k) * *(B + k*(cols2+1) + j);
            }
        }
    }
}

@implementation CompassBearing

@synthesize bearing, bearingDegrees;

- (id)init
{
    if(self = [super init])
	{
		bearing = bearingDegrees = 0;
		useDeclination = NO;
		declination = 0;
		ax=ay=az=cx=cy=cz=0;
		filterConstant = 0.1;
	}
	
    return self;
}

- (void)setAccelerationVector:(double)x:(double)y:(double)z
{
	ax=x;
	ay=y;
	az=z;
}
- (void)setMagneticFieldVector:(double)x:(double)y:(double)z
{
	cx=x;
	cy=y;
	cz=z;
}
- (void)setDeclination:(double)d
{
	declination=d*M_PI/180.0; //store in rads
}

- (void)setUseDeclination:(Boolean)state
{
	useDeclination = state;
}

-(void)setSampleRate:(double)rate cutoffFrequency:(double)freq
{
	double dt = 1.0 / rate;
	double RC = 1.0 / freq;
	filterConstant = dt / (dt + RC);
}

//This finds a bearing, correcting for board tilt and roll as measured by the accelerometer
//This doesn't account for dynamic acceleration - ie accelerations other then gravity will throw off the calculation
// but we can help by feeding it low-pass filtered data
- (void)updateBearing
{
	double Xh = 0;
	double Yh = 0;
	
	//find the tilt of the board wrt gravity
	double gravity[4][2] = {0};
	gravity[1][1] = ax;
	gravity[2][1] = az;
	gravity[3][1] = ay;
	normalize(gravity);
	
	double pitchAngle = asin(gravity[1][1]);
	double rollAngle = asin(gravity[3][1]);
	
	//The board is up-side down
	if (gravity[2][1] < 0)
	{
		pitchAngle = -pitchAngle;
		rollAngle = -rollAngle;
	}
	
	//Construct a rotation matrix for rotating vectors measured in the body frame, into the earth frame
	//this is done by using the angles between the board and the gravity vector.
	double xRotMatrix[4][4] = {0};
	xRotMatrix[1][1] = cos(pitchAngle); xRotMatrix[2][1] = -sin(pitchAngle); xRotMatrix[3][1] = 0;
	xRotMatrix[1][2] = sin(pitchAngle); xRotMatrix[2][2] = cos(pitchAngle); xRotMatrix[3][2] = 0;
	xRotMatrix[1][3] = 0; xRotMatrix[2][3] = 0; xRotMatrix[3][3] = 1;
	
	double zRotMatrix[4][4] = {0};
	zRotMatrix[1][1] = 1; zRotMatrix[2][1] = 0; zRotMatrix[3][1] = 0;
	zRotMatrix[1][2] = 0; zRotMatrix[2][2] = cos(rollAngle); zRotMatrix[3][2] = -sin(rollAngle);
	zRotMatrix[1][3] = 0; zRotMatrix[2][3] = sin(rollAngle); zRotMatrix[3][3] = cos(rollAngle);
	
	double rotMatrix[4][4] = {0};
	crossProduct((double *)xRotMatrix, (double *)zRotMatrix, (double *)rotMatrix, 3, 3, 3, 3);
	
	//These represent the x and y components of the magnetic field vector in the earth frame
	Xh = -(rotMatrix[1][3] * cx + rotMatrix[2][3] * cz + rotMatrix[3][3] * -cy);
	Yh = -(rotMatrix[1][1] * cx + rotMatrix[2][1] * cz + rotMatrix[3][1] * -cy);
	
	//we use the computed X-Y to find a magnetic North bearing in the earth frame
	double _360inRads = (360 * M_PI / 180.0);
	double newBearing;
	if (Xh < 0)
		newBearing = M_PI - atan(Yh / Xh);
	else if (Xh > 0 && Yh < 0)
		newBearing = -atan(Yh / Xh);
	else if (Xh > 0 && Yh > 0)
		newBearing = M_PI * 2 - atan(Yh / Xh);
	else if (Xh == 0 && Yh < 0)
		newBearing = M_PI / 2.0;
	else if (Xh == 0 && Yh > 0)
		newBearing = M_PI * 1.5;
	
	//The board is up-side down
	if (gravity[2][1] < 0)
	{
		newBearing = fabs(newBearing - _360inRads);
	}
	
	//Add in declination
	if(useDeclination)
	{
		newBearing = (newBearing + declination);
		if(newBearing > _360inRads)
			newBearing -= _360inRads;
		if(newBearing < 0)
			newBearing += _360inRads;
	}
	
	if (fabs(newBearing - bearing) > 2) //2 radians == ~115 degrees
	{
		if(newBearing > bearing)
			bearing += _360inRads;
		else
			bearing -= _360inRads;
	}
	
	bearing = newBearing * filterConstant + bearing * (1.0 - filterConstant);
	
	bearingDegrees = bearing * (180.0 / M_PI);
}

@end
