/* VectorNav HSI Calibration Library v1.0.0.2
 *
 * Copyright (c) 2018 VectorNav Technologies, LLC
 *
 * This source code is proprietary to and copyrighted by VectorNav Technologies, LLC and is
 * available solely under license from VectorNav. All rights reserved. If you do not have a
 * license to use this source code from VectorNav, any use hereof is strictly prohibited by
 * U.S. law and other applicable law. This source code is restricted for use solely with the
 * products of VectorNav and as otherwise set forth in any agreement with VectorNav. Any
 * disclosure of this source code to the public or to any third party is also prohibited.
 */
#include "vnhsidipole.h"
#include "vnamath.h"

/* Values defined by WMM2015 */
#define DIPOLE_B0				0.312f	/* field strength in Gauss */
#define DIPOLE_MAG_NORTH_LAT	80.37f	/* latitude of north geomagnetic pole */
#define DIPOLE_MAG_MORTH_LONG	-72.62f	/* longitude of north geomagnetic pole */

/** Estimate local Earth's magnetic field based on a simple dipole model.
 * \param[in] latitude geocentric latitude (deg)
 * \param[in] longitude longitude (deg)
 * \param[out] bnd_v2 two component float vector containing (Bnorth, Bdown).
 *     	Bnorth is north component of Earth's magnetic field; 
 *		Bdown (positive down) component of Earth's magnetic field. */
static void vnhsi_dipole_magnetic_model(float latitude, float longitude, float* bnd_v2);

static VnHsiDipole dipole0;
static VnHsiDipole dipole;

VnHsiDipole vnhsi_dipole_init(VnHsiConfig* c)
{
	float BND[2];

	vnhsi_dipole_magnetic_model(c->latitude*VN_PIF/180.0f, c->longitude*VN_PIF/180.0f,  BND);
	dipole.Bdown = BND[0]; 	
	dipole.Bnorth = BND[1]; 
	dipole.sphereRad = sqrtf(dipole.Bdown*dipole.Bdown + dipole.Bnorth*dipole.Bnorth);

	dipole0 = dipole;
	
	return dipole0;
}

VnHsiDipole vnhsi_dipole_get_dipole(bool updatedFlag)
{
	if(updatedFlag)
		return dipole;
	else
		return dipole0;
}

void vnhsi_dipole_update(float Bdown, bool updateMagnitude)
{
	dipole.Bdown = Bdown;
	if(updateMagnitude)
		dipole.sphereRad = sqrtf(dipole.Bnorth*dipole.Bnorth + dipole.Bdown*dipole.Bdown);
	else
		dipole.Bnorth = sqrtf(dipole.sphereRad*dipole.sphereRad - dipole.Bdown*dipole.Bdown);
}

static void vnhsi_dipole_magnetic_model(float latitude, float longitude, float* bnd_v2)
{
	float mnlat, mnlong;	
	float maglat, AA = 0;
	union vn_vec3f magNorthVec, posVec;
	int c1;
	
	mnlat = vn_deg2rad(DIPOLE_MAG_NORTH_LAT);
	mnlong = vn_deg2rad(DIPOLE_MAG_MORTH_LONG);	

	/* Calculate magnetic vector */
	vn_init_v3(magNorthVec.c,
		cosf(mnlat) * cosf(mnlong),
		cosf(mnlat) * sinf(mnlong),
		sinf(mnlat));
	posVec.c[0] = cosf(latitude) * cosf(longitude);
	posVec.c[1] = cosf(latitude) * sinf(longitude);
	posVec.c[2] = sinf(latitude);
	
	for (c1=0; c1<3; c1++) {
		AA += posVec.c[c1] * magNorthVec.c[c1];
	}	
	
	maglat = acosf(AA);

	bnd_v2[0] = 2.0f * DIPOLE_B0 * cosf(maglat);
	bnd_v2[1] = DIPOLE_B0 * sinf(maglat);
}