/* 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.
 */
#ifndef VNAMATH_H_INCLUDED
#define VNAMATH_H_INCLUDED

#include "vnenum.h"

/* TODO: This should probably be moved to its own file. */
#define VN_PI	3.1415926535
#define VN_PIF	((float)VN_PI)

#ifdef __cplusplus
extern "C" {
#endif

/** TODO: DEOXYGEN GROUP
  * Explain the general structure of each function instead of
  * writing each and every one out. */

float vn_sign(float v);

void vn_init_v2(float* r, float c0, float c1);
void vn_init_v3(float* r, float c0, float c1, float c2);
void vn_init_v4(float* r, float c0, float c1, float c2, float c3);
void vn_init_m2(float* r, float e00, float e01, float e10, float e11);
void vn_init_m3(float* r, float e00, float e01, float e02, float e10, float e11, float e12, float e20, float e21, float e22);
void vn_init_m4(float* r, float e00, float e01, float e02, float e03, float e10, float e11, float e12, float e13, float e20, float e21, float e22, float e23, float e30, float e31, float e32, float e33);

/** Negates a matrix value stored in a float array.
  * \param[in] A an m x n matrix of float values
  * \param[in] m m-dimension of A
  * \param[in] n n-dimension of A
  * \param[out] r result of negation (m x n matrix)
  */
void vn_neg_m(const float* A, size_t m, size_t n, float* r);

void vn_neg_m3(const float* A, float* r);														/**< Negates 3x3 float matrix. */

void vn_mult_vs(const float* a, float s, size_t n, float* r);									/**< Multiply an n-dimensional float vector by a scalar. */
void vn_mult_v2s(const float* a, float s, float* r);											/**< Multiply an 2-dimensional float vector by a scalar. */
void vn_mult_v3s(const float* a, float s, float* r);											/**< Multiply an 3-dimensional float vector by a scalar. */
void vn_mult_v4s(const float* a, float s, float* r);											/**< Multiply an 4-dimensional float vector by a scalar. */
void vn_mult_mn(const float* A, const float* B, size_t m, size_t p, size_t n, float* r);		/**< Multiply mxp x pxn float matrices. */
void vn_mult_m2_m2(const float* A, const float* B, float* r);									/**< Multiply two 2x2 float matrices together. */
void vn_mult_m3_m3(const float* A, const float* B, float* r);									/**< Multiply two 3x3 float matrices together. */
void vn_mult_m4_m4(const float* A, const float* B, float* r);									/**< Multiply two 4x4 float matrices together. */
void vn_mult_m3_v3(const float* A, const float* b, float* r);									/**< Multiply 3-dim vector b by 3x3 matrix A and store in 3-dim vector r. */

void vn_add_v(const float* a, const float* b, size_t n, float* r);								/**< Add 2 n-dimensional float matrices. */
void vn_add_v2(const float* a, const float* b, float* r);										/**< Add 2 2-dimensional float matrices. */
void vn_add_v3(const float* a, const float* b, float* r);										/**< Add 2 3-dimensional float matrices. */
void vn_add_v4(const float* a, const float* b, float* r);										/**< Add 2 4-dimensional float matrices. */
void vn_add_m(const float* A, const float* B, size_t m, size_t n, float* r);					/**< Add 2 mxn matrices. */
void vn_add_m2(const float* A, const float* B, float* r);										/**< Add 2 2x2 matrices. */
void vn_add_m3(const float* A, const float* B, float* r);										/**< Add 2 3x3 matrices. */
void vn_add_m4(const float* A, const float* B, float* r);										/**< Add 2 4x4 matrices. */

void vn_sub_m(const float* A, const float* B, size_t m, size_t n, float* r);					/**< Subtract mxn matrix B from mxn matrix A. */
void vn_sub_m2(const float* A, const float* B, float* r);										/**< Subtract 2x2 matrix B from 2x2 matrix A. */
void vn_sub_m3(const float* A, const float* B, float* r);										/**< Subtract 3x3 matrix B from 3x3 matrix A. */
void vn_sub_m4(const float* A, const float* B, float* r);										/**< Subtract 4x4 matrix B from 4x4 matrix A. */

void vn_set_m(float* A, size_t m, size_t n, size_t i, size_t j, float val);						/**< Sets val at position i,j in mxn matrix A. */
void vn_set_m2(float* A, size_t i, size_t j, float val);										/**< Sets val at position i,j in 2x2 matrix A. */
void vn_set_m3(float* A, size_t i, size_t j, float val);										/**< Sets val at position i,j in 3x3 matrix A. */
void vn_set_m4(float* A, size_t i, size_t j, float val);										/**< Sets val at position i,j in 4x4 matrix A. */

float vn_get_m(float* A, size_t m, size_t n, size_t i, size_t j);								/**< Gets value at position i,j in mxn matrix A. */
float vn_get_m2(float* A, size_t i, size_t j);													/**< Gets value at position i,j in 2x2 matrix A. */
float vn_get_m3(float* A, size_t i, size_t j);													/**< Gets value at position i,j in 3x3 matrix A. */
float vn_get_m4(float* A, size_t i, size_t j);													/**< Gets value at position i,j in 4x4 matrix A. */

/* TODO: Why is vn_scale_ms not simply vn_mult_ms??? *? */
void vn_scale_ms(const float* A, const float b, size_t m, size_t n, float* r);						/**< Scale mxn matrix A by scalar b. */

void vn_tranpose_m(const float* A, size_t m, size_t n, float* r);								/**< Transpose of mxn matrix, */
void vn_tranpose_m2(const float* A, float* r);													/**< Transpose of 2x2 matrix. */
void vn_tranpose_m3(const float* A, float* r);													/**< Transpose of 3x3 matrix. */
void vn_tranpose_m4(const float* A, float* r);													/**< Transpose of 4x4 matrix. */

void vn_zero_m(float* A, size_t m, size_t n);													/**< Sets all elements in the mxn matrix to zero. */
void vn_zero_m2(float* A);																		/**< Sets all elements in the 2x2 matrix to zero. */
void vn_zero_m3(float* A);																		/**< Sets all elements in the 3x3 matrix to zero. */
void vn_zero_m4(float* A);																		/**< Sets all elements in the 4x4 matrix to zero. */
void vn_zero_v(float* a, size_t n);																/**< Sets all elements in the n-dimensional matrix to zero. */
void vn_zero_v2(float* a);																		/**< Sets all elements in the 2-dimensional matrix to zero. */
void vn_zero_v3(float* a);																		/**< Sets all elements in the 3-dimensional matrix to zero. */
void vn_zero_v4(float* a);																		/**< Sets all elements in the 4-dimensional matrix to zero. */

void vn_eye_m(float* A, size_t m);																/**< Constructs an mxm identity matrix. */
void vn_eye_mn(float* A, size_t m, size_t n);													/**< Constructs an mxn identity matrix. */
void vn_eye_m2(float* A);																		/**< Constructs a 2x2 identity matrix. */
void vn_eye_m3(float* A);																		/**< Constructs a 3x3 identity matrix. */
void vn_eye_m4(float* A);																		/**< Constructs a 4x4 identity matrix. */

enum VnError vn_inverse_m(float* A, size_t n, float* r);												/**< Inverse of nxn matrix. */
enum VnError vn_inverse_m2(float* A, float* r);														/**< Inverse of 2x2 matrix. */
enum VnError vn_inverse_m3(float* A, float* r);														/**< Inverse of 3x3 matrix. */
enum VnError vn_inverse_m4(float* A, float* r);														/**< Inverse of 4x4 matrix. */

float vn_norm_v(float* a, size_t n);
void vn_normalize_v(float* a, size_t n, float* r);												/**< Normalizes the n-dimensional float vector a. */
void vn_normalize_v2(float* a, float* r);														/**< Normalizes the 2-dimensional float vector a. */
void vn_normalize_v3(float* a, float* r);														/**< Normalizes the 3-dimensional float vector a. */
void vn_normalize_v4(float* a, float* r);														/**< Normalizes the 4-dimensional float vector a. */

/* TODO: Should this be renamed to just vn_cross() ???. */
void vn_cross_v3(float* a, float* b, float* r);													/**< Cross product of two 3-dimensional float vectors. */
void vn_cross_norm_v3(float* a, float* b, float* r);
float vn_dot(float* a, float* b);

void vn_copy_v(const float* a, size_t d, float* r);												/**< Copy n dimensional float vector to another. */
void vn_copy_v2(const float* a, float* r);														/**< Copy 2 dimensional float vector to another. */
void vn_copy_v3(const float* a, float* r);														/**< Copy 3 dimensional float vector to another. */
void vn_copy_v4(const float* a, float* r);														/**< Copy 4 dimensional float vector to another. */
void vn_copy_m(const float* a, size_t m, size_t n, float* r);									/**< Copy mxn dimensional float matrix to another. */
void vn_copy_m2(const float* a, float* r);														/**< Copy 2x2 dimensional float matrix to another. */
void vn_copy_m3(const float* a, float* r);														/**< Copy 3x3 dimensional float matrix to another. */
void vn_copy_m4(const float* a, float* r);														/**< Copy 4x4 dimensional float matrix to another. */

void vn_to_column_order_m(const float* a, size_t m, size_t n, float* r);
void vn_to_column_order_m2(const float* a, float* r);
void vn_to_column_order_m3(const float* a, float* r);
void vn_to_column_order_m4(const float* a, float* r);

/* TODO: This should probably be moved to its own file. */
float vn_deg2rad(float deg);
double vn_deg2rad_d(double deg);

float vn_cond(float *A, float *Ainv, size_t n);
float vn_std(float* samp, size_t n);
float vn_mean(float* samp, size_t n);
float vn_abs_max(float* samp, size_t n);

#ifdef __cplusplus
}
#endif

#endif