/*
    2017: Distributed by Insight SiP
	
	THIS SOFTWARE IS PROVIDED BY INSIGHT SIP "AS IS" AND ANY EXPRESS
	OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
	OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
	DISCLAIMED. IN NO EVENT SHALL INSIGHT SIP OR CONTRIBUTORS BE
	LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
	GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
	OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/



#include "app_util_platform.h"
#include "nrf_error.h"
#include <string.h>
#include <stdint.h>
#include <math.h>
#include "arm_math.h"
#include "arm_const_structs.h"
#include "matrix.h"
#include "orientation.h"

static float fBx[MAG_BUFF_SIZE][MAG_BUFF_SIZE][MAG_BUFF_SIZE];				// array of x magnetic fields
static float fBy[MAG_BUFF_SIZE][MAG_BUFF_SIZE][MAG_BUFF_SIZE];				// array of y magnetic fields
static float fBz[MAG_BUFF_SIZE][MAG_BUFF_SIZE][MAG_BUFF_SIZE];				// array of z magnetic fields

static int calibration_index[MAG_BUFF_SIZE][MAG_BUFF_SIZE][MAG_BUFF_SIZE]; 	// array of time indices
uint16_t calibration_loop_counter = 0;
uint16_t m_mag_buffer_count = 0;



// magnetometer calibration reset function
void reset_mag_calibration()
{
    int j, k, l;		// loop counters

    // set magnetic buffer index to invalid value -1 to denote invalid
	for (j = 0; j < MAG_BUFF_SIZE; j++)
	{
		for (k = 0; k < MAG_BUFF_SIZE; k++)
		{
			for (l = 0; l < MAG_BUFF_SIZE; l++)
			{
				calibration_index[j][k][l] = -1;
			}
		}
	}
}

uint16_t m_mag_buffer_count_get(void)
{
	return m_mag_buffer_count;
}

uint32_t orient_calculate_rotation_matrix (float rotation_matrix[3][3], float MX, float MY, float MZ, float GX, float GY, float GZ)
{
    float fmodB, fmodG, fmodx, fmody;

	// place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes
	rotation_matrix[0][2] = GX;  
	rotation_matrix[1][2] = GY;
	rotation_matrix[2][2] = GZ;
	rotation_matrix[0][0] = MX;
	rotation_matrix[1][0] = MY;
	rotation_matrix[2][0] = MZ;

	// set y vector to vector product of z and x vectors
	rotation_matrix[0][1] = rotation_matrix[1][2] * rotation_matrix[2][0] - rotation_matrix[2][2] * rotation_matrix[1][0];
	rotation_matrix[1][1] = rotation_matrix[2][2] * rotation_matrix[0][0] - rotation_matrix[0][2] * rotation_matrix[2][0];
	rotation_matrix[2][1] = rotation_matrix[0][2] * rotation_matrix[1][0] - rotation_matrix[1][2] * rotation_matrix[0][0];

	// set x vector to vector product of y and z vectors
	rotation_matrix[0][0] = rotation_matrix[1][1] * rotation_matrix[2][2] - rotation_matrix[2][1] * rotation_matrix[1][2];
	rotation_matrix[1][0] = rotation_matrix[2][1] * rotation_matrix[0][2] - rotation_matrix[0][1] * rotation_matrix[2][2];
	rotation_matrix[2][0] = rotation_matrix[0][1] * rotation_matrix[1][2] - rotation_matrix[1][1] * rotation_matrix[0][2];

    // calculate the vector moduli
	fmodG = (float) sqrt ( GX * GX + GY * GY + GZ * GZ);
	fmodB = (float) sqrt ( MX * MX + MY * MY + MZ * MZ);
	fmodx = (float) sqrt ( rotation_matrix[0][0] * rotation_matrix[0][0] +
						   rotation_matrix[1][0] * rotation_matrix[1][0] +
						   rotation_matrix[2][0] * rotation_matrix[2][0]);
	fmody = (float) sqrt(  rotation_matrix[0][1] * rotation_matrix[0][1] +
						   rotation_matrix[1][1] * rotation_matrix[1][1] +
						   rotation_matrix[2][1] * rotation_matrix[2][1]);
	
	// normalize the rotation matrix
	if (!((fmodx == 0.0F) || (fmody == 0.0F) || (fmodG == 0.0F)))
	{
		// normalize x axis
		rotation_matrix[0][0] /= fmodx;
		rotation_matrix[1][0] /= fmodx;
		rotation_matrix[2][0] /= fmodx;
		// normalize y axis
		rotation_matrix[0][1] /= fmody;
		rotation_matrix[1][1] /= fmody;
		rotation_matrix[2][1] /= fmody;
		// normalize z axis
		rotation_matrix[0][2] /= fmodG;
		rotation_matrix[1][2] /= fmodG;
		rotation_matrix[2][2] /= fmodG;
	}
	// no solution is possible to set rotation to identity matrix and delta to 0 degrees
	else
	{
		fmatrix3_A_eq_I(rotation_matrix);
	}
	
	return NRF_SUCCESS;
}

 // extract the NED angles in degrees from the NED rotation matrix
uint32_t orient_calculate_euler_angles (float euler[3], float rotation_matrix[3][3])
{
	// calculate the roll angle range -180.0 <= Phi < 180.0 deg
	euler[0] = (float) atan2(rotation_matrix[1][2], rotation_matrix[2][2]) * F_RAD_TO_DEG;
	// map +180 roll onto the functionally equivalent -180 deg roll
	if (euler[0] == 180.0F)
		euler[0] = -180.0F;	

	// calculate the pitch angle -90.0 <= Theta <= 90.0 deg
	if (rotation_matrix[0][2] >= 1.0F)
		euler[1] = -90.0F;
	else if (rotation_matrix[0][2] <= -1.0F)
		euler[1] = 90.0F;
	else
		euler[1] = (float) asin(-rotation_matrix[0][2]) * F_RAD_TO_DEG;	
	
	// calculate the yaw and compass angle 0.0 <= Psi < 360.0 deg
    euler[2] =  (float) atan2(rotation_matrix[0][1], rotation_matrix[0][0]) * F_RAD_TO_DEG;     
	if (euler[2] < 0.0F)
		euler[2] += 360.0F;
	// check for rounding errors mapping small negative angle to 360 deg
	if (euler[2] >= 360.0F)
		euler[2] = 0.0F;	
	
	return NRF_SUCCESS;
}

 // compute the orientation quaternion from a rotation matrix
uint32_t orient_calculate_quaternions (float quaternion[4], float rotation_matrix[3][3])
{
	float fq0sq;			// q0^2
	float recip4q0;			// 1/4q0
	float fmag;             // quaternion magnitude
	
   // get q0^2 and q0
	fq0sq = 0.25F * (1.0F + rotation_matrix[0][0] + rotation_matrix[1][1] + rotation_matrix[2][2]);
	quaternion[0] = (float) sqrt(fabs(fq0sq));	
	
	// normal case when q0 is not small meaning rotation angle not near 180 deg
	if (quaternion[0] > SMALLQ0) 
	{
		// calculate q1 to q3
		recip4q0 = 0.25F / quaternion[0];
		quaternion[1] = recip4q0 * (rotation_matrix[1][2] - rotation_matrix[2][1]);
		quaternion[2] = recip4q0 * (rotation_matrix[2][0] - rotation_matrix[0][2]);
		quaternion[3] = recip4q0 * (rotation_matrix[0][1] - rotation_matrix[1][0]);
	}
	else 
	{
		// special case of near 180 deg corresponds to nearly symmetric matrix
		// which is not numerically well conditioned for division by small q0
		// instead get absolute values of q1 to q3 from leading diagonal
		quaternion[1] = (float) sqrt(fabs(0.5F * (1.0F + rotation_matrix[0][0]) - fq0sq));
		quaternion[2] = (float) sqrt(fabs(0.5F * (1.0F + rotation_matrix[1][1]) - fq0sq));
		quaternion[3] = (float) sqrt(fabs(0.5F * (1.0F + rotation_matrix[2][2]) - fq0sq));

		// first assume q1 is positive and ensure q2 and q3 are consistent with q1
		if ((rotation_matrix[0][1] + rotation_matrix[1][0]) < 0.0F) 
		{
			// q1*q2 < 0 so q2 is negative
			quaternion[2] = -quaternion[2];
			if ((rotation_matrix[1][2] + rotation_matrix[2][1]) > 0.0F) 
			{
				// q1*q2 < 0 and q2*q3 > 0 so q3 is also both negative
				quaternion[3] = -quaternion[3];
			}
		} else if ((rotation_matrix[0][1] + rotation_matrix[1][0]) > 0.0F) 
		{
			if ((rotation_matrix[1][2] + rotation_matrix[2][1]) < 0.0F) 
			{
				// q1*q2 > 0 and q2*q3 < 0 so q3 is negative
				quaternion[3] = -quaternion[3];
			}
		}

		// negate the vector components if q1 should be negative
		if ((rotation_matrix[1][2] - rotation_matrix[2][1]) < 0.0F) {
			quaternion[1] = -quaternion[1];
			quaternion[2] = -quaternion[2];
			quaternion[3] = -quaternion[3];
		}
	}	
	
	 // finally re-normalize for safety
	fmag = (float) sqrt(quaternion[0] * quaternion[0] + quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]);
	if (fmag > SMALLQ0) {
		// normal case
		quaternion[0] /= fmag;
		quaternion[1] /= fmag;
		quaternion[2] /= fmag;
		quaternion[3] /= fmag;
	} else {
		// severe rounding errors so return identity quaternion
		quaternion[0] = 1.0F;
		quaternion[1] = quaternion[2] = quaternion[3] = 0.0F;
	}
	
	return NRF_SUCCESS;	
}

uint32_t update_magnetometer_buffer(float MX, float MY, float MZ, float GX, float GY, float GZ)
{
	// local variables
	int j, k, l;					// magnetic buffer indices
	int previndex;					// previous time index in the bin being over-written
	int oldestj, oldestk, oldestl;	// indices of bin with oldest data
	int oldestindex;				// time index of bin with oldest data
	float ftmp;						// scratch variable

	// map -90 to 90 degrees onto range 0 to MAGBUFFSIZE - 1
	ftmp = (float) MAG_BUFF_SIZE * F_RECIP_180;
	j = (int) ((atan2(GX, fabs(GY)) * F_RAD_TO_DEG + 90.0F) * ftmp);
	k = (int) ((atan2(GY, fabs(GZ)) * F_RAD_TO_DEG + 90.0F) * ftmp);
	l = (int) ((atan2(GZ, fabs(GX)) * F_RAD_TO_DEG + 90.0F) * ftmp);

	// bounds safety check in case exactly on boundary
	if (j >= MAG_BUFF_SIZE) j = MAG_BUFF_SIZE - 1;
	if (k >= MAG_BUFF_SIZE) k = MAG_BUFF_SIZE - 1;
	if (l >= MAG_BUFF_SIZE) l = MAG_BUFF_SIZE - 1;

	// save the previous time index in this bin for later use
	previndex = calibration_index[j][k][l];

	// store the magnetometer reading
	fBx[j][k][l] = MX;
	fBy[j][k][l] = MY;
	fBz[j][k][l] = MZ;


	// set the time index to the loop counter (valid value 0 at start of first pass)
	calibration_index[j][k][l] = calibration_loop_counter;

	// no additional action needed if current bin was active
	// but action is needed if bin was inactive (value -1)
	if (previndex == -1)
	{
		if (m_mag_buffer_count < MAX_EQUATIONS)
		{
			// simply increase the count of active measurements for calibration
			(m_mag_buffer_count)++;
		}
		else
		{
			// we need to find and de-activate the oldest reading
			oldestindex = calibration_loop_counter;
			oldestj = oldestk = oldestl = 0; // to avoid compiler error
			for (j = 0; j < MAG_BUFF_SIZE; j++)
			{
				for (k = 0; k < MAG_BUFF_SIZE; k++)
				{
					for (l = 0; l < MAG_BUFF_SIZE; l++)
					{
						if ((calibration_index[j][k][l] != -1) && (calibration_index[j][k][l] < oldestindex))
						{
							oldestj = j;
							oldestk = k;
							oldestl = l;
							oldestindex = calibration_index[oldestj][oldestk][oldestl];
						}
					}
				}
			}
			// deactivate the oldest reading without bothering to zero the measurement data
			calibration_index[oldestj][oldestk][oldestl] = -1;
		}
	}
	calibration_loop_counter++;
	
	return NRF_SUCCESS;
}

uint32_t hard_iron_calibration(float *x_offset, float *y_offset, float *z_offset)
{
	uint16_t j, k, l;	
	uint16_t nb_x = 0, nb_y = 0, nb_z = 0;
	float32_t buffer[MAG_BUFF_SIZE*MAG_BUFF_SIZE*MAG_BUFF_SIZE];
	float32_t min, max;
	
	// X axe
	memset(buffer, 0, MAG_BUFF_SIZE*MAG_BUFF_SIZE*MAG_BUFF_SIZE*sizeof(float));
	for (j = 0; j < MAG_BUFF_SIZE; j++)
	{
		for (k = 0; k < MAG_BUFF_SIZE; k++)
		{
			for (l = 0; l < MAG_BUFF_SIZE; l++)
			{
				if(fBx[j][k][l] != 0.0F)
				{
					buffer[nb_x] = fBx[j][k][l];
					nb_x++;
				}		
			}
		}
	}
	arm_min_f32((float32_t*)buffer, nb_x, &min, NULL);
	arm_max_f32((float32_t*)buffer, nb_x, &max, NULL);
	*x_offset = (min + max) / 2;
	
	// Y axe
	memset(buffer, 0, MAG_BUFF_SIZE*MAG_BUFF_SIZE*MAG_BUFF_SIZE*sizeof(float));
	for (j = 0; j < MAG_BUFF_SIZE; j++)
	{
		for (k = 0; k < MAG_BUFF_SIZE; k++)
		{
			for (l = 0; l < MAG_BUFF_SIZE; l++)
			{
				if(fBy[j][k][l] != 0.0F)
				{
					buffer[nb_y] = fBy[j][k][l];
					nb_y++;
				}		
			}
		}
	}	
	arm_min_f32((float32_t*)buffer, nb_y, &min, NULL);
	arm_max_f32((float32_t*)buffer, nb_y, &max, NULL);
	*y_offset = (min + max) / 2;
	
	// Z axe
	memset(buffer, 0, MAG_BUFF_SIZE*MAG_BUFF_SIZE*MAG_BUFF_SIZE*sizeof(float));
	for (j = 0; j < MAG_BUFF_SIZE; j++)
	{
		for (k = 0; k < MAG_BUFF_SIZE; k++)
		{
			for (l = 0; l < MAG_BUFF_SIZE; l++)
			{
				if(fBz[j][k][l] != 0.0F)
				{
					buffer[nb_z] = fBz[j][k][l];
					nb_z++;
				}		
			}
		}
	}		
	arm_min_f32((float32_t*)buffer, nb_z, &min, NULL);
	arm_max_f32((float32_t*)buffer, nb_z, &max, NULL);
	*z_offset = (min + max) / 2;
	
	return NRF_SUCCESS;
}





