/*
    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.
*/

#ifndef ORIENTATION_H
#define ORIENTATION_H


#define SMALLQ0			0.01F		            // limit where rounding errors may appear

 // multiplicative conversion constants
//#define  PI 					3.141592654F			// Pi
#define  F_DEG_TO_RAD 			0.01745329251994F 		// degrees to radians conversion = pi / 180
#define  F_RAD_TO_DEG 			57.2957795130823F		// radians to degrees conversion = 180 / pi
#define  F_RECIP_180 			0.0055555555555F		// multiplicative factor 1/180
#define  F_MATRIX_SCALING 		0.02F		    		// approx normalizes geomagnetic field 50uT
#define  F_INV_MATRIX_SCALING 	50.0F					// inverse of FMATRIXSCALING

// magnetic calibration constants
#define MAG_BUFF_SIZE		6				// Size of magnetometer buffer
#define MIN_EQUATIONS  		70		//24    // minimum number of measurements used for calibration
#define MED_EQUATIONS  		100		//80    // new solution always accepted up to this point
#define MAX_EQUATIONS  		120            	// maximum number of measurements used for calibration
#define MIN_B_FIT 			5.0F            // minimum acceptable geomagnetic field B (uT) for valid calibration
#define MAX_B_FIT 			100.0F          // maximum acceptable geomagnetic field B (uT) for valid calibration

// sensor scaling constants
#define F_COUNTS_PER_G 		16393.0f
#define F_COUNTS_PER_UT  	81.9f 
#define F_UT_PER_COUNTS  	0.01221f

void reset_mag_calibration(void);

/**@brief return m_mag_buffer_count
 *
 */
uint16_t m_mag_buffer_count_get(void);

/**@brief Calculate the rotation matrix
 *
 * @param[out]  rotation_matrix 3x3 rotation table
 * @param[in]   MX 				X component of magnetization
 * @param[in]   MY 				Y component of magnetization
 * @param[in]   MZ 				Z component of magnetization
 * @param[in]   GX 				X component of acceleration
 * @param[in]   GY 				Y component of acceleration
 * @param[in]   GZ 				Z component of acceleration
 *
 * @return NRF_SUCCESS    If the call was successful.
 */
uint32_t orient_calculate_rotation_matrix (float rotation_matrix[3][3], float MX, float MY, float MZ, float GX, float GY, float GZ);

/**@brief Calculate the euler angles using rotation matrix
 *
 * @param[out] euler 			euler angle : Phi, Theta, Yaw
 * @param[in]  rotation_matrix 	3x3 rotation table
 *
 * @return NRF_SUCCESS    If the call was successful.
 */
uint32_t orient_calculate_euler_angles (float euler[3], float rotation_matrix[3][3]);

/**@brief Calculate the quaternion using rotation matrix
 *
 * @param[out] quaternion		quaternions : q0, q1, q2, q3
 * @param[in]  rotation_matrix 	3x3 rotation table
 *
 * @return NRF_SUCCESS    If the call was successful.
 */
uint32_t orient_calculate_quaternions (float quaternion[4], float rotation_matrix[3][3]);

/**@brief Function for update magnetometer buffer for calibration
 *
 * @param[in]   MX 				X component of magnetization
 * @param[in]   MY 				Y component of magnetization
 * @param[in]   MZ 				Z component of magnetization
 * @param[in]   GX 				X component of acceleration
 * @param[in]   GY 				Y component of acceleration
 * @param[in]   GZ 				Z component of acceleration
 *
 * @return NRF_SUCCESS    If the call was successful.
 */
uint32_t update_magnetometer_buffer(float MX, float MY, float MZ, float GX, float GY, float GZ);

/**@brief Function for calibrating hard iron offset
 *
 * @param[in]   x_offset 		X magnetization offset
 * @param[in]   y_offset 		Y magnetization offset
 * @param[in]   z_offset 		Z magnetization offset
 */
uint32_t hard_iron_calibration(float *x_offset, float *y_offset, float *z_offset);

#endif
