/* 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 "vnhsipatches.h"

#ifdef MAXPATCHES

static VnHsiPatchFill northPatches;
static VnHsiPatchFill eastPatches;
static VnHsiPatchDef patchdef;

static int vnhsi_find_patch(VnHsiConfig* c, float vec[3], unsigned int threshFlag);

static const float Patches32[32][3] = {
	{                  0.0f,   0.356822089819834f,   0.934172358944861f},
	{                  0.0f,  -0.356822089819834f,   0.934172358944861f},
	{                  0.0f,   0.356822089819834f,  -0.934172358944861f},
	{                  0.0f,  -0.356822089819834f,  -0.934172358944861f},
	{   0.934172358944861f,                  0.0f,   0.356822089819834f},
	{   0.934172358944861f,                  0.0f,  -0.356822089819834f},
	{  -0.934172358944861f,                  0.0f,   0.356822089819834f},
	{  -0.934172358944861f,                  0.0f,  -0.356822089819834f},
	{   0.356822089819834f,   0.934172358944861f,                  0.0f},
	{  -0.356822089819834f,   0.934172358944861f,                  0.0f},
	{   0.356822089819834f,  -0.934172358944861f,                  0.0f},
	{  -0.356822089819834f,  -0.934172358944861f,                  0.0f},
	{   0.577350269189626f,   0.577350269189626f,   0.577350269189626f},
	{   0.577350269189626f,   0.577350269189626f,  -0.577350269189626f},
	{   0.577350269189626f,  -0.577350269189626f,   0.577350269189626f},
	{   0.577350269189626f,  -0.577350269189626f,  -0.577350269189626f},
	{  -0.577350269189626f,   0.577350269189626f,   0.577350269189626f},
	{  -0.577350269189626f,   0.577350269189626f,  -0.577350269189626f},
	{  -0.577350269189626f,  -0.577350269189626f,   0.577350269189626f},
	{  -0.577350269189626f,  -0.577350269189626f,  -0.577350269189626f},
	{   0.85065080840000f,	 0.52573111210000f,	 0.00000000000000f},
	{  -0.85065080840000f,	 0.52573111210000f,	 0.00000000000000f},
	{  -0.85065080840000f,	-0.52573111210000f,	 0.00000000000000f},
	{   0.85065080840000f,	-0.52573111210000f,	 0.00000000000000f},
	{   0.52573111210000f,	 0.00000000000000f,	 0.85065080840000f},
	{   0.52573111210000f,	 0.00000000000000f,	-0.85065080840000f},
	{  -0.52573111210000f,	 0.00000000000000f,	-0.85065080840000f},
	{  -0.52573111210000f,	 0.00000000000000f,	 0.85065080840000f},
	{   0.00000000000000f,	 0.85065080840000f,	 0.52573111210000f},
	{   0.00000000000000f,	-0.85065080840000f,	 0.52573111210000f},
	{   0.00000000000000f,	-0.85065080840000f,	-0.52573111210000f},
	{   0.00000000000000f,	 0.85065080840000f,	-0.52573111210000f}

};

static const float Patches50[50][3] = {
	{    0.862856209461017f,    0.357406744336593f,    0.357406744336593f},
	{    0.357406744336593f,    0.357406744336593f,    0.862856209461017f},
	{    0.357406744336593f,    0.862856209461017f,    0.357406744336593f},
	{    0.577350269189626f,    0.577350269189626f,    0.577350269189626f},
	{   -0.357406744336593f,    0.862856209461017f,    0.357406744336593f},
	{   -0.357406744336593f,    0.357406744336593f,    0.862856209461017f},
	{   -0.862856209461017f,    0.357406744336593f,    0.357406744336593f},
	{   -0.577350269189626f,    0.577350269189626f,    0.577350269189626f},
	{   -0.862856209461017f,   -0.357406744336593f,    0.357406744336593f},
	{   -0.357406744336593f,   -0.357406744336593f,    0.862856209461017f},
	{   -0.357406744336593f,   -0.862856209461017f,    0.357406744336593f},
	{   -0.577350269189626f,   -0.577350269189626f,    0.577350269189626f},
	{    0.357406744336593f,   -0.862856209461017f,    0.357406744336593f},
	{    0.357406744336593f,   -0.357406744336593f,    0.862856209461017f},
	{    0.862856209461017f,   -0.357406744336593f,    0.357406744336593f},
	{    0.577350269189626f,   -0.577350269189626f,    0.577350269189626f},
	{    0.862856209461017f,    0.357406744336593f,   -0.357406744336593f},
	{    0.357406744336593f,    0.862856209461017f,   -0.357406744336593f},
	{    0.357406744336593f,    0.357406744336593f,   -0.862856209461017f},
	{    0.577350269189626f,    0.577350269189626f,   -0.577350269189626f},
	{   -0.357406744336593f,    0.862856209461017f,   -0.357406744336593f},
	{   -0.862856209461017f,    0.357406744336593f,   -0.357406744336593f},
	{   -0.357406744336593f,    0.357406744336593f,   -0.862856209461017f},
	{   -0.577350269189626f,    0.577350269189626f,   -0.577350269189626f},
	{   -0.862856209461017f,   -0.357406744336593f,   -0.357406744336593f},
	{   -0.357406744336593f,   -0.862856209461017f,   -0.357406744336593f},
	{   -0.357406744336593f,   -0.357406744336593f,   -0.862856209461017f},
	{   -0.577350269189626f,   -0.577350269189626f,   -0.577350269189626f},
	{    0.357406744336593f,   -0.862856209461017f,   -0.357406744336593f},
	{    0.862856209461017f,   -0.357406744336593f,   -0.357406744336593f},
	{    0.357406744336593f,   -0.357406744336593f,   -0.862856209461017f},
	{    0.577350269189626f,   -0.577350269189626f,   -0.577350269189626f},
	{   1.00000000000000f,	 0.00000000000000f,	 0.00000000000000f},
	{  -1.00000000000000f,	 0.00000000000000f,	 0.00000000000000f},
	{   0.00000000000000f,	 1.00000000000000f,	 0.00000000000000f},
	{   0.00000000000000f,	-1.00000000000000f,	 0.00000000000000f},
	{   0.00000000000000f,	 0.00000000000000f,	 1.00000000000000f},
	{   0.00000000000000f,	 0.00000000000000f,	-1.00000000000000f},
	{   0.70710678118655f,	 0.00000000000000f,	 0.70710678118655f},
	{   0.00000000000000f,	 0.70710678118655f,	 0.70710678118655f},
	{   0.70710678118655f,	 0.70710678118655f,	 0.00000000000000f},
	{  -0.70710678118655f,	 0.00000000000000f,	 0.70710678118655f},
	{  -0.70710678118655f,	 0.70710678118655f,	 0.00000000000000f},
	{   0.00000000000000f,	-0.70710678118655f,	 0.70710678118655f},
	{  -0.70710678118655f,	-0.70710678118655f,	 0.00000000000000f},
	{   0.70710678118655f,	-0.70710678118655f,	 0.00000000000000f},
	{   0.00000000000000f,	 0.70710678118655f,	-0.70710678118655f},
	{   0.70710678118655f,	 0.00000000000000f,	-0.70710678118655f},
	{  -0.70710678118655f,	 0.00000000000000f,	-0.70710678118655f},
	{   0.00000000000000f,	-0.70710678118655f,	-0.70710678118655f}
}; 

static const float Patches122[122][3] = {
	{ 0.268034881851658f, 0.194738740691547f, 0.943522191039849f},
	{ 0.000000000000000f, 0.628428289746823f, 0.777867523840585f},
	{-0.268034881851658f, 0.194738740691547f, 0.943522191039849f},
	{ 0.000000000000000f, 0.356822089767573f, 0.934172358964823f},
	{ 0.268034881851658f,-0.194738740691547f, 0.943522191039849f},
	{-0.268034881851658f,-0.194738740691547f, 0.943522191039849f},
	{ 0.000000000000000f,-0.628428289746823f, 0.777867523840585f},
	{ 0.000000000000000f,-0.356822089767573f, 0.934172358964823f},
	{ 0.268034881851658f, 0.194738740691547f,-0.943522191039849f},
	{-0.268034881851658f, 0.194738740691547f,-0.943522191039849f},
	{ 0.000000000000000f, 0.628428289746823f,-0.777867523840585f},
	{ 0.000000000000000f, 0.356822089767573f,-0.934172358964823f},
	{ 0.268034881851658f,-0.194738740691547f,-0.943522191039849f},
	{ 0.000000000000000f,-0.628428289746823f,-0.777867523840585f},
	{-0.268034881851658f,-0.194738740691547f,-0.943522191039849f},
	{ 0.000000000000000f,-0.356822089767573f,-0.934172358964823f},
	{ 0.943522191039849f, 0.268034881851658f, 0.194738740691547f},
	{ 0.777867523840585f, 0.000000000000000f, 0.628428289746823f},
	{ 0.943522191039849f,-0.268034881851658f, 0.194738740691547f},
	{ 0.934172358964823f, 0.000000000000000f, 0.356822089767573f},
	{ 0.943522191039849f, 0.268034881851658f,-0.194738740691547f},
	{ 0.943522191039849f,-0.268034881851658f,-0.194738740691547f},
	{ 0.777867523840585f, 0.000000000000000f,-0.628428289746823f},
	{ 0.934172358964823f, 0.000000000000000f,-0.356822089767572f},
	{-0.943522191039849f,-0.268034881851658f, 0.194738740691547f},
	{-0.777867523840585f, 0.000000000000000f, 0.628428289746823f},
	{-0.943522191039849f, 0.268034881851658f, 0.194738740691547f},
	{-0.934172358964823f, 0.000000000000000f, 0.356822089767573f},
	{-0.943522191039849f,-0.268034881851658f,-0.194738740691547f},
	{-0.943522191039849f, 0.268034881851658f,-0.194738740691547f},
	{-0.777867523840585f, 0.000000000000000f,-0.628428289746823f},
	{-0.934172358964823f, 0.000000000000000f,-0.356822089767573f},
	{ 0.194738740691547f, 0.943522191039849f, 0.268034881851658f},
	{ 0.628428289746823f, 0.777867523840585f,-0.000000000000000f},
	{ 0.194738740691547f, 0.943522191039849f,-0.268034881851658f},
	{ 0.356822089767573f, 0.934172358964823f,-0.000000000000000f},
	{-0.194738740691547f, 0.943522191039849f, 0.268034881851658f},
	{-0.194738740691547f, 0.943522191039849f,-0.268034881851658f},
	{-0.628428289746823f, 0.777867523840585f,-0.000000000000000f},
	{-0.356822089767573f, 0.934172358964823f,-0.000000000000000f},
	{ 0.194738740691547f,-0.943522191039849f, 0.268034881851658f},
	{ 0.194738740691547f,-0.943522191039849f,-0.268034881851658f},
	{ 0.628428289746823f,-0.777867523840585f,-0.000000000000000f},
	{ 0.356822089767573f,-0.934172358964823f,-0.000000000000000f},
	{-0.194738740691547f,-0.943522191039849f, 0.268034881851658f},
	{-0.628428289746823f,-0.777867523840585f,-0.000000000000000f},
	{-0.194738740691547f,-0.943522191039849f,-0.268034881851658f},
	{-0.356822089767573f,-0.934172358964823f,-0.000000000000000f},
	{ 0.315093901330349f, 0.748783450381823f, 0.583128783184908f},
	{ 0.583128783184908f, 0.315093901330349f, 0.748783450381823f},
	{ 0.748783450381823f, 0.583128783184908f, 0.315093901330349f},
	{ 0.577350269189626f, 0.577350269189626f, 0.577350269189626f},
	{ 0.315093901330349f, 0.748783450381823f,-0.583128783184908f},
	{ 0.748783450381823f, 0.583128783184908f,-0.315093901330349f},
	{ 0.583128783184908f, 0.315093901330350f,-0.748783450381823f},
	{ 0.577350269189626f, 0.577350269189626f,-0.577350269189626f},
	{ 0.583128783184908f,-0.315093901330349f, 0.748783450381823f},
	{ 0.315093901330349f,-0.748783450381823f, 0.583128783184908f},
	{ 0.748783450381823f,-0.583128783184908f, 0.315093901330349f},
	{ 0.577350269189626f,-0.577350269189626f, 0.577350269189626f},
	{ 0.583128783184908f,-0.315093901330350f,-0.748783450381824f},
	{ 0.748783450381823f,-0.583128783184908f,-0.315093901330349f},
	{ 0.315093901330349f,-0.748783450381823f,-0.583128783184908f},
	{ 0.577350269189626f,-0.577350269189626f,-0.577350269189626f},
	{-0.583128783184908f, 0.315093901330349f, 0.748783450381823f},
	{-0.315093901330349f, 0.748783450381823f, 0.583128783184908f},
	{-0.748783450381823f, 0.583128783184908f, 0.315093901330349f},
	{-0.577350269189626f, 0.577350269189626f, 0.577350269189626f},
	{-0.583128783184908f, 0.315093901330349f,-0.748783450381823f},
	{-0.748783450381823f, 0.583128783184908f,-0.315093901330349f},
	{-0.315093901330349f, 0.748783450381823f,-0.583128783184908f},
	{-0.577350269189626f, 0.577350269189626f,-0.577350269189626f},
	{-0.583128783184908f,-0.315093901330349f, 0.748783450381823f},
	{-0.748783450381823f,-0.583128783184908f, 0.315093901330349f},
	{-0.315093901330349f,-0.748783450381823f, 0.583128783184908f},
	{-0.577350269189626f,-0.577350269189626f, 0.577350269189626f},
	{-0.583128783184908f,-0.315093901330349f,-0.748783450381823f},
	{-0.315093901330349f,-0.748783450381823f,-0.583128783184908f},
	{-0.748783450381823f,-0.583128783184908f,-0.315093901330349f},
	{-0.577350269189626f,-0.577350269189626f,-0.577350269189626f},
	{   0.85065080837385f,	 0.52573111208384f,	 0.00000000000000f},
	{  -0.85065080837385f,	 0.52573111208384f,	 0.00000000000000f},
	{  -0.85065080837385f,	-0.52573111208384f,	 0.00000000000000f},
	{   0.85065080837385f,	-0.52573111208384f,	 0.00000000000000f},
	{   0.52573111208384f,	 0.00000000000000f,	 0.85065080837385f},
	{   0.52573111208384f,	 0.00000000000000f,	-0.85065080837385f},
	{  -0.52573111208384f,	 0.00000000000000f,	-0.85065080837385f},
	{  -0.52573111208384f,	 0.00000000000000f,	 0.85065080837385f},
	{   0.00000000000000f,	 0.85065080837385f,	 0.52573111208384f},
	{   0.00000000000000f,	-0.85065080837385f,	 0.52573111208384f},
	{   0.00000000000000f,	-0.85065080837385f,	-0.52573111208384f},
	{   0.00000000000000f,	 0.85065080837385f,	-0.52573111208384f},
	{   0.30901699435618f,	 0.50000000001603f,	 0.80901699437221f},
	{  -0.30901699435618f,	 0.50000000001603f,	 0.80901699437221f},
	{   0.00000000000000f,	 0.00000000000000f,	 1.00000000000000f},
	{  -0.30901699435618f,	-0.50000000001603f,	 0.80901699437221f},
	{   0.30901699435618f,	-0.50000000001603f,	 0.80901699437221f},
	{   0.00000000000000f,	 0.00000000000000f,	-1.00000000000000f},
	{  -0.30901699435618f,	 0.50000000001603f,	-0.80901699437221f},
	{   0.30901699435618f,	 0.50000000001603f,	-0.80901699437221f},
	{   0.30901699435618f,	-0.50000000001603f,	-0.80901699437221f},
	{  -0.30901699435618f,	-0.50000000001603f,	-0.80901699437221f},
	{   0.80901699437221f,	 0.30901699435618f,	 0.50000000001603f},
	{   0.80901699437221f,	-0.30901699435618f,	 0.50000000001603f},
	{   1.00000000000000f,	 0.00000000000000f,	 0.00000000000000f},
	{   0.80901699437221f,	-0.30901699435618f,	-0.50000000001603f},
	{   0.80901699437221f,	 0.30901699435618f,	-0.50000000001603f},
	{  -0.80901699437221f,	-0.30901699435618f,	 0.50000000001603f},
	{  -0.80901699437221f,	 0.30901699435618f,	 0.50000000001603f},
	{  -1.00000000000000f,	 0.00000000000000f,	 0.00000000000000f},
	{  -0.80901699437221f,	 0.30901699435618f,	-0.50000000001603f},
	{  -0.80901699437221f,	-0.30901699435618f,	-0.50000000001603f},
	{   0.50000000001603f,	 0.80901699437221f,	 0.30901699435618f},
	{   0.50000000001603f,	 0.80901699437221f,	-0.30901699435618f},
	{   0.00000000000000f,	 1.00000000000000f,	 0.00000000000000f},
	{  -0.50000000001603f,	 0.80901699437221f,	-0.30901699435618f},
	{  -0.50000000001603f,	 0.80901699437221f,	 0.30901699435618f},
	{   0.00000000000000f,	-1.00000000000000f,	 0.00000000000000f},
	{   0.50000000001603f,	-0.80901699437221f,	-0.30901699435618f},
	{   0.50000000001603f,	-0.80901699437221f,	 0.30901699435618f},
	{  -0.50000000001603f,	-0.80901699437221f,	 0.30901699435618f},
	{  -0.50000000001603f,	-0.80901699437221f,	-0.30901699435618f}

	
}; 

static const float Patches212[212][3] = {
	{  -0.86712330178929f,	 0.38963182862975f,	 0.31029698292553f},
	{  -0.84343414985279f,	 0.27170960000478f,	 0.46345736387217f},
	{  -0.49014654224124f,	 0.78736092627394f,	-0.37392397476761f},
	{  -0.61380450795881f,	 0.58946773401309f,	-0.52513980668667f},
	{  -0.97748631895403f,	-0.09588538675093f,	 0.18795342206337f},
	{  -0.92953869719237f,	-0.35571829270265f,	 0.09706856678975f},
	{  -0.81102274820271f,	-0.15565255196967f,	 0.56392764160223f},
	{  -0.89620578986319f,	-0.02928048904578f,	 0.44267124954863f},
	{  -0.99413955927664f,	 0.10806493723095f,	-0.00291650861952f},
	{  -0.95595431472627f,	 0.29129458909585f,	 0.03603901385583f},
	{  -0.85500769983422f,	 0.48948111167720f,	-0.17137699535076f},
	{  -0.82250772325605f,	 0.44233767987351f,	-0.35751702357828f},
	{   0.93721545449849f,	-0.14426210799798f,	-0.31751478082944f},
	{   0.92223539327491f,	-0.31467936692917f,	-0.22463030832935f},
	{   0.55650389594392f,	 0.78855404554934f,	 0.26169816783273f},
	{   0.31751811063112f,	 0.94552926991063f,	-0.07180981244582f},
	{   0.49023550816823f,	 0.87123612335195f,	-0.02483473168162f},
	{   0.97758923012252f,	 0.09544782066882f,	-0.18764064239399f},
	{  -0.66751454891192f,	 0.32530194009490f,	 0.66977830269531f},
	{  -0.51283252857853f,	 0.55598767809583f,	 0.65412575200596f},
	{  -0.34342310027504f,	 0.57520469506425f,	 0.74242853728391f},
	{  -0.21485637172962f,	 0.35436337546655f,	 0.91008974153934f},
	{   0.13060964683147f,	-0.02795581194232f,	 0.99103965245252f},
	{  -0.14817872450534f,	-0.04669235935335f,	 0.98785772719658f},
	{  -0.29439871994258f,	 0.18335262357439f,	 0.93792921328028f},
	{  -0.61774681378356f,	-0.07212220321446f,	 0.78306274452546f},
	{  -0.56736291778386f,	 0.11071005887205f,	 0.81599179063783f},
	{   0.59733955742336f,	-0.40075371506353f,	 0.69468115923785f},
	{   0.52544403661124f,	-0.14436348886227f,	 0.83849135205630f},
	{   0.24515128145127f,	-0.17411692486559f,	 0.95372120961953f},
	{  -0.43654076176487f,	-0.30978847492064f,	 0.84466754650815f},
	{  -0.26170623759132f,	-0.29247997687053f,	 0.91976372418987f},
	{   0.45103996183873f,	-0.29505344453189f,	-0.84232203918358f},
	{   0.66750736297704f,	-0.32526583549354f,	-0.66980299837500f},
	{   0.75590455058785f,	 0.08093415005656f,	-0.64965989083151f},
	{   0.80240192838517f,	-0.09818014625390f,	-0.58865253265855f},
	{   0.72757830053597f,	-0.40947651635125f,	 0.55041693210333f},
	{   0.93657393241258f,	-0.34171156546028f,	 0.07786189797277f},
	{  -0.50619581652898f,	-0.65717705417754f,	-0.55846585821435f},
	{  -0.02930153696988f,	-0.16914434570682f,	-0.98515562742473f},
	{  -0.35685819058412f,	-0.51299121080240f,	-0.78069984594114f},
	{  -0.71141909420387f,	 0.64762183932986f,	 0.27288976826763f},
	{  -0.57932251886331f,	 0.71122418385323f,	 0.39817782389301f},
	{  -0.70327033162484f,	 0.71044465650024f,	-0.02606205491768f},
	{  -0.56773181677897f,	 0.81614771550286f,	-0.10762662633526f},
	{   0.64980287296554f,	 0.73188882076015f,	-0.20517061273984f},
	{   0.83132080558532f,	 0.54619536178972f,	-0.10284135821922f},
	{   0.80861979416446f,	 0.33215587515636f,	-0.48559911767273f},
	{   0.89038818306426f,	 0.33010393682821f,	-0.31343304604023f},
	{   0.04348969201021f,	 0.30242923405745f,	-0.95217918748326f},
	{   0.14805510834928f,	 0.04749364258201f,	-0.98783806304777f},
	{   0.52555448987901f,	 0.13788856088915f,	-0.83951130006923f},
	{   0.41127204740490f,	 0.00127890522883f,	-0.91151174837453f},
	{   0.61019652856446f,	 0.54505413891443f,	-0.57495754815475f},
	{   0.55380950045422f,	 0.69304287115320f,	-0.46150473014950f},
	{   0.43479875771088f,	 0.42595075895356f,	-0.79342043787639f},
	{   0.25872299164995f,	 0.48861367167898f,	-0.83325811933643f},
	{  -0.13916972497142f,	 0.70933862538281f,	-0.69099240385941f},
	{  -0.10811091033855f,	 0.94153779618991f,	-0.31909028410720f},
	{  -0.23368768675900f,	 0.86480838814330f,	-0.44440580200332f},
	{   0.13790070547763f,	 0.70110364614799f,	-0.69959779358340f},
	{   0.19332167354301f,	 0.91736220230546f,	-0.34795591720764f},
	{   0.27914401669922f,	 0.82327653301707f,	-0.49426143701934f},
	{  -0.25157863244926f,	 0.57759524365624f,	-0.77658993439308f},
	{  -0.53067616280898f,	 0.50676826401044f,	-0.67938850212392f},
	{  -0.14133792808483f,	 0.33747105471908f,	-0.93066475022506f},
	{  -0.95860662260057f,	-0.00854483278850f,	-0.28460556730140f},
	{  -0.86797174752545f,	 0.18876438273646f,	-0.45933980157149f},
	{  -0.12038849661941f,	 0.75780590527810f,	 0.64127749048860f},
	{   0.34064464460778f,	 0.83649502751511f,	 0.42922872112957f},
	{   0.69340087719072f,	 0.65509307738689f,	 0.30008046166139f},
	{   0.85390824589799f,	 0.51384470211999f,	 0.08248836094050f},
	{  -0.51938584375884f,	-0.53983534362907f,	 0.66243199429964f},
	{  -0.75069675282436f,	-0.44438249125809f,	 0.48885436150475f},
	{  -0.79269155821883f,	-0.52004735024170f,	 0.31810508803726f},
	{   0.14373273471773f,	-0.45420349925512f,	 0.87922697992894f},
	{  -0.04127099247442f,	-0.49884428557493f,	 0.86570842893518f},
	{   0.36292226894277f,	-0.59257110043585f,	 0.71912927741378f},
	{   0.70903143577460f,	-0.62273693408223f,	 0.33086724530130f},
	{   0.78796062649432f,	-0.59541981175342f,	 0.15682250752427f},
	{   0.67032489112303f,	-0.54677003288577f,	-0.50170416729283f},
	{   0.76503938260340f,	-0.55076809473738f,	-0.33372031536177f},
	{   0.68124003783227f,	-0.72544760790249f,	-0.09817219078155f},
	{   0.53009067121065f,	-0.84182444625131f,	-0.10166357257694f},
	{  -0.66538586925130f,	-0.57739682768182f,	-0.47315383162736f},
	{  -0.74727532979025f,	-0.63744735172302f,	-0.18772441308521f},
	{  -0.91243748177806f,	-0.40028179802790f,	-0.08504307152300f},
	{  -0.92945701024599f,	-0.19515259826632f,	-0.31308965089011f},
	{  -0.77617433116171f,	-0.30763822469844f,	-0.55037453642957f},
	{  -0.65001634267576f,	-0.75653347284180f,	-0.07166490580726f},
	{  -0.64951887780313f,	-0.73213715711432f,	 0.20518384573335f},
	{  -0.47534338121457f,	-0.27013883167073f,	-0.83730142813629f},
	{  -0.69725580204004f,	-0.19483557315110f,	-0.68983581086834f},
	{  -0.31616564473911f,	-0.06870922708670f,	-0.94621262261706f},
	{  -0.35602204461282f,	 0.11919551218555f,	-0.92684450347646f},
	{  -0.59437628601494f,	 0.22470985172927f,	-0.77215433247434f},
	{  -0.72312118534150f,	 0.10913580550885f,	-0.68204481323901f},
	{  -0.13546432167414f,	 0.86494047575902f,	 0.48324671850632f},
	{  -0.36560629188247f,	 0.87219448059014f,	 0.32497511806913f},
	{   0.09384734192461f,	 0.98723805219439f,	 0.12866120127340f},
	{   0.10906393522694f,	 0.94221371356962f,	 0.31676233361017f},
	{  -0.17161310922643f,	 0.98489465785129f,	-0.02305327910810f},
	{  -0.33741441414825f,	 0.93966612203445f,	 0.05638343928605f},
	{   0.31623877825889f,	 0.68361900512280f,	 0.65776750524804f},
	{   0.14305881307330f,	 0.64992422369197f,	 0.74641320959670f},
	{   0.08704393349284f,	 0.40265623774996f,	 0.91120321983800f},
	{   0.21641419020203f,	 0.26243069447020f,	 0.94036962354124f},
	{   0.60556505194753f,	 0.02932531399878f,	 0.79525530103147f},
	{   0.47528587487775f,	 0.27003502090298f,	 0.83736755646945f},
	{   0.68905552622569f,	 0.47808739563637f,	 0.54464201445671f},
	{   0.54679701180835f,	 0.48654836483578f,	 0.68138367793269f},
	{   0.86786489998562f,	-0.18899367274331f,	 0.45944739311039f},
	{   0.81553291414256f,	 0.05733902432312f,	 0.57586309331283f},
	{   0.84510031900710f,	 0.22622679077464f,	 0.48438299923708f},
	{   0.96144700105628f,	-0.19231347574532f,	 0.19655836590347f},
	{   0.99133616819267f,	 0.09466105015203f,	 0.09105980022590f},
	{   0.94917940832081f,	 0.24496727804470f,	 0.19760942160516f},
	{  -0.13806373703069f,	-0.70132770712900f,	 0.69934101247554f},
	{  -0.39968438484590f,	-0.68328004971736f,	 0.61104890652762f},
	{  -0.28400668806847f,	-0.92285674673924f,	 0.26014539421302f},
	{  -0.43353034991429f,	-0.83234791475587f,	 0.34532359390108f},
	{   0.49041528068439f,	-0.78700195862974f,	 0.37432708903337f},
	{   0.32839069709107f,	-0.73523043865040f,	 0.59295510129012f},
	{   0.19344076794941f,	-0.96992939835831f,	 0.14770792631215f},
	{   0.37547243189323f,	-0.91412204662387f,	 0.15297495469641f},
	{  -0.00324144211119f,	-0.92641469589481f,	 0.37649077582746f},
	{   0.04439765674065f,	-0.83898525966385f,	 0.54233991383884f},
	{   0.36529468023677f,	-0.87205206956119f,	-0.32570689978069f},
	{   0.42173286752632f,	-0.72052184119561f,	-0.55044496982724f},
	{   0.03115063622390f,	-0.99328055212515f,	-0.11146022892856f},
	{   0.10226170639594f,	-0.95337918246636f,	-0.28392019626080f},
	{  -0.07606617885150f,	-0.85490739041798f,	-0.51317374274572f},
	{  -0.34051416742457f,	-0.83659893881562f,	-0.42912972089536f},
	{  -0.26297848677545f,	-0.96399865718959f,	-0.03935612315720f},
	{  -0.39835284445581f,	-0.90212168833940f,	-0.16580552084181f},
	{   0.30731551214472f,	-0.42101704117810f,	-0.85340601534842f},
	{   0.28965200492504f,	-0.67240147658797f,	-0.68115928410705f},
	{   0.02262913451375f,	-0.34552044382096f,	-0.93813834010391f},
	{  -0.16670675599343f,	-0.55082979415166f,	-0.81779911676462f},
	{  -0.00412822081307f,	-0.75092866947888f,	-0.66037041964915f},
	{   0.33963110518701f,	 0.92168297276218f,	 0.18748655981091f},
	{  -0.76381622154265f,	 0.08988884300386f,	 0.63914378320667f},
	{   0.82095321699761f,	-0.33174169664738f,	-0.46473999419761f},
	{   0.60572841289870f,	-0.78221928833341f,	 0.14569171138539f},
	{  -0.72495238366666f,	 0.35727703037317f,	-0.58889486751352f},
	{  -0.26294860769728f,	-0.71908927971630f,	-0.64324850369601f},
	{  -0.23702020596601f,	-0.32015670533106f,	-0.91723557824334f},
	{   0.58286136647621f,	-0.73794374331537f,	-0.34016416503091f},
	{   0.32125163968257f,	 0.47409212740115f,	 0.81977682251788f},
	{  -0.58952240050721f,	-0.43673141185453f,	-0.67950644824001f},
	{   0.61850466100331f,	-0.09899343928721f,	-0.77952054706420f},
	{   0.43825997484276f,	 0.85274041287113f,	-0.28418652801847f},
	{   0.38470337571554f,	 0.64525297204511f,	-0.66003932820705f},
	{   0.64907276031546f,	 0.31520042863473f,	-0.69235340802581f},
	{  -0.71355010985666f,	 0.50131767644346f,	 0.48941478115079f},
	{  -0.94277977651404f,	 0.24892926862557f,	-0.22181188475298f},
	{  -0.34319478935779f,	 0.91589394672546f,	-0.20822011168311f},
	{  -0.56112025502406f,	-0.76265335383428f,	-0.32172025315026f},
	{   0.38740485173333f,	-0.36569527018663f,	 0.84627681654209f},
	{  -0.69405962595061f,	 0.66196344657881f,	-0.28299404767380f},
	{   0.18861067700830f,	-0.84032920647312f,	-0.50820550692287f},
	{   0.23350146027684f,	-0.19125084262735f,	-0.95336256652069f},
	{  -0.03257016980358f,	 0.18157672091330f,	 0.98283725939819f},
	{  -0.55223210231977f,	-0.02567766079395f,	-0.83329488352182f},
	{  -0.09363280949945f,	 0.56710010509073f,	 0.81830945722955f},
	{  -0.98666585520483f,	-0.15048616442857f,	-0.06200326191817f},
	{  -0.00295320780451f,	-0.25634483661337f,	 0.96658088296082f},
	{  -0.85092171396945f,	-0.05907333168151f,	-0.52196032241861f},
	{   0.85756999551254f,	-0.40319028547989f,	 0.31939207330688f},
	{  -0.80605286004644f,	-0.58857041083072f,	 0.06215833255086f},
	{   0.72236785270141f,	-0.17267043343537f,	 0.66960406719254f},
	{   0.93895471863346f,	 0.02773098563556f,	 0.34292131574407f},
	{   0.49672332085296f,	-0.51393552682345f,	-0.69938273984244f},
	{   0.69448044583819f,	 0.71799816264272f,	 0.04664277854137f},
	{  -0.47397230609785f,	-0.87624006165946f,	 0.08691149173335f},
	{  -0.45869132723173f,	 0.35256335466850f,	 0.81566006845210f},
	{  -0.16320152936194f,	-0.94604075196979f,	-0.27995027491746f},
	{  -0.10500940195910f,	 0.08541687508754f,	-0.99079613591822f},
	{  -0.38740484486854f,	 0.36569525573682f,	-0.84627682592871f},
	{  -0.00148110939414f,	 0.53993791342851f,	-0.84170354398531f},
	{   0.30065126937309f,	 0.24224535984767f,	-0.92245650296186f},
	{   0.83526611774716f,	 0.43821300770524f,	 0.33211424603819f},
	{  -0.38579477480794f,	 0.70647944494350f,	-0.59333732867840f},
	{   0.25457061524938f,	-0.88596992136232f,	 0.38762236815334f},
	{   0.75301078684411f,	 0.55298290603918f,	-0.35662958447790f},
	{  -0.89523839774926f,	-0.27730165189201f,	 0.34878647487152f},
	{   0.28896682770794f,	-0.95261125233765f,	-0.09502617746756f},
	{  -0.06674661775705f,	-0.98913147492185f,	 0.13101074130359f},
	{  -0.95495213493142f,	 0.14984948763041f,	 0.25614751813522f},
	{   0.11371631007292f,	-0.66945306431964f,	 0.73409890035093f},
	{  -0.29639086365149f,	-0.52550053004113f,	 0.79749711527404f},
	{   0.68110460944448f,	 0.25823732293408f,	 0.68513502029697f},
	{   0.11711282106649f,	 0.82420608505659f,	 0.55404775651340f},
	{   0.56188798016063f,	-0.61649161423326f,	 0.55156140848602f},
	{  -0.14274290209991f,	 0.96121885122585f,	 0.23597199822894f},
	{   0.94832171554628f,	 0.31249366499248f,	-0.05507842738247f},
	{   0.99041679670198f,	-0.12516047796415f,	-0.05839026944937f},
	{   0.53121433016411f,	 0.68177075001697f,	 0.50299103356779f},
	{   0.37568762915498f,	 0.05091151075839f,	 0.92534686651666f},
	{   0.09363279472490f,	-0.56710010160705f,	-0.81830946133433f},
	{  -0.55162290937426f,	 0.81911572999262f,	 0.15735814796866f},
	{  -0.39365371475176f,	-0.06218935303659f,	 0.91715278837881f},
	{   0.89770115948704f,	 0.09224752042227f,	-0.43083990440947f},
	{  -0.60612412480038f,	-0.65822990671215f,	 0.44648284988866f},
	{  -0.83526594462995f,	-0.43821311257614f,	-0.33211454305370f},
	{  -0.64907275853935f,	-0.31520042246767f,	 0.69235341249851f},
	{  -0.84737963617090f,	 0.52377314721483f,	 0.08723211827959f},
	{   0.84737962882655f,	-0.52377315984698f,	-0.08723211377523f},
	{  -0.36552776586425f,	 0.75072432711859f,	 0.55027487408988f},
	{   0.06674662641243f,	 0.98913147486380f,	-0.13101073733218f},
	{  -0.21712679249228f,	-0.83926255431212f,	 0.49849204698922f},
	{   0.01742563573491f,	 0.84876186459378f,	-0.52848807406651f}
}; 

static float Patches2D[MAXPATCHES][3];
static int lastPatch;

void vnhsi_patches_init(VnHsiConfig* c) 
{
	unsigned int c1;
	float dotAngle;
	float hFOV;
	
	VnHsiDipole dipole;
	
	if(c->mode == HSI_MODE_3D || c->mode == HSI_MODE_3D_NOLOCATION){ /* 3D mode */
		switch (c->nPatches)
		{
			case 32 :
				dotAngle =  37.377368f; 
				patchdef.Patches = Patches32;
				break;
			case 50 :
				dotAngle =  24.374416f; 
				patchdef.Patches = Patches50;
				break;
			case 122 :
				dotAngle = 18.029102f;
				patchdef.Patches = Patches122;
				break;
			case 212 :
				dotAngle = 13.5565412f;
				patchdef.Patches = Patches212;
				break;
			default :
				c->nPatches = 122;
				dotAngle = 18.029102109f; 
				patchdef.Patches = Patches122;
				break;
		}
		
		hFOV = 0.5f*(dotAngle)*VN_PIF/180.0f;
	} else if(c->mode == HSI_MODE_2D || c->mode == HSI_MODE_2D_NOLOCATION || c->mode == HSI_MODE_2D_SCALE_S33){ /* 2D mode*/
		if(c->nPatches > MAXPATCHES)
			c->nPatches = MAXPATCHES;
		dipole = vnhsi_get_dipole(false);
		hFOV = VN_PIF/(float)c->nPatches;
		for(c1=0;c1<c->nPatches;c1++) {
			Patches2D[c1][0] = dipole.Bnorth*cosf((float)c1*2.0f*hFOV);
			Patches2D[c1][1] = dipole.Bnorth*sinf((float)c1*2.0f*hFOV);
			Patches2D[c1][2] = dipole.Bdown;
		}
		patchdef.Patches = (const float(*)[3])Patches2D;
	}
	
	patchdef.cosErr = cosf(hFOV);
	
	/* initialize patch flags */
	for(c1=0;c1<c->nPatches;c1++) {
		northPatches.numData[c1]= 0;
		eastPatches.numData[c1]= 0;
	}
	
	patchdef.refYPR.c[0] = patchdef.refYPR.c[1] = patchdef.refYPR.c[2] = 0.0f;
}

VnHsiPatch vnhsi_patches_adddata(VnHsiDipole dipole, VnHsiConfig* c, union vn_vec3f ypr, int patchtype)
{
	VnHsiPatch patch;
	VnHsiPatchFill* patchFillPtr;
	union vn_mat3f DCM;
	union vn_vec3f bm, e;
	union vn_vec3f EastVec;
	unsigned int threshFlag;
	union vn_vec3f magRefNorm = vnhsi_unit_mag_ref(dipole);
	
	EastVec.c[0] = EastVec.c[2] = 0.0f;	EastVec.c[1] = 1.0f;
	
	vn_mult_v3s(ypr.c,VN_PIF/180.0f,e.c);
	Euler3212C(DCM.e, e.c);

	if(patchtype == NORTHPATCHTYPE) {
	/* calculate the body mag. reference  vector from the ypr */
		patchFillPtr = &northPatches;
		vn_mult_mn((float*) DCM.e, magRefNorm.c, 3, 3, 1, bm.c);
		threshFlag = 1;
	}
	else if(patchtype == EASTPATCHTYPE) {
		/* calculate the body mag. reference  vector from the ypr */
		patchFillPtr = &eastPatches;
		vn_mult_mn((float*) DCM.e, EastVec.c, 3, 3, 1, bm.c);
		threshFlag = 1;
	}
	else if(patchtype == TWODPATCHTYPE) {
		patchFillPtr = &northPatches;
		vn_mult_mn((float*) DCM.e, magRefNorm.c, 3, 3, 1, bm.c);
		threshFlag = 0;
	}
	else {
		patch.patchInd = -1;
		patch.numData = 0;
		return patch;
	}
	
	patch.patchInd = vnhsi_find_patch(c, bm.c, threshFlag);
	
	if(patch.patchInd >= 0) {
		patch.numData = patchFillPtr->numData[patch.patchInd];
		if(patch.numData < c->nDataPerPatch) {
			patchFillPtr->numData[patch.patchInd]++;
			if(patchtype != EASTPATCHTYPE)
				lastPatch = patch.patchInd;
		} else {
			patch.patchInd = -1;
			patch.numData = 0;
			return patch;
		}
			
		if(patch.numData==1)
			patchFillPtr->numFilled++;
	} else {
		patch.numData = 0;
	}
		
	return patch;

}

int vnhsi_find_patch(VnHsiConfig* c, float vec[3], unsigned int threshFlag)
{
	unsigned int c1,c2;
	float dotP;
	float dotPmax = 0.0f;
	
	int patchind = -1;
	
	for(c1=0;c1<c->nPatches;c1++) {
		/* calculate dot-product of vector with patch-vector */
		dotP = 0.0f;
		for(c2=0;c2<3;c2++)
			dotP += patchdef.Patches[c1][c2]*vec[c2];
		
		/* if dot-product greater than threshold, correct patch found - return immediately */
		if(dotP >= patchdef.cosErr)
			return c1;
		else if(threshFlag == 0 && dotP > dotPmax) { /* find max dot-product if thresholding turned off */
				dotPmax = dotP;
				patchind = c1;
		}
	}
	return patchind;
}

VnHsiPatchFill vnhsi_patches_fill(VnHsiPatchType patchType)
{
	if(patchType == EASTPATCHTYPE)
		return eastPatches;
	else
		return northPatches;
}

VnHsiPatchDef vnhsi_patches_def()
{	
	return patchdef;
}

void vnhsi_patches_set_refYPR(union vn_vec3f ypr)
{
	patchdef.refYPR = ypr;
}

int vnhsi_patches_last_patch()
{
	return lastPatch;
}

#endif /* MAXPATCHES */