/********************************************************************************

INTEL CORPORATION PROPRIETARY INFORMATION This software is supplied under the
terms of a license agreement or nondisclosure agreement with Intel Corporation
and may not be copied or disclosed except in accordance with the terms of that
agreement.
Copyright(c) 2011-2016 Intel Corporation. All Rights Reserved.

 *********************************************************************************/

#pragma once
namespace rs
{
namespace slam
{

/**
 * Linear algebra utility for 4X4 dimension Matrix.
 * Matrix data is stored row major order, Memory layout
 *      __                      __
 *     | 0x0    0x1    0x2    0x3 |
 *     | 1x0    1x1    1x2    1x3 |
 *     | 2x0    2x1    2x2    2x3 |
 *     |_3x0    3x1    3x2    3x3_|
 */
class Matrix4f
{
public:

    /**
     * @brief: Storage for 4X4 matrix elements.
     */
    float m_data[16];

    /**
     * @brief: Default constructor initialized all elements of Matrix with 0 value.
     */
    Matrix4f();

    /**
     * @brief: Default constructor initialized all elements of Matrix with specified value.
     */
    explicit Matrix4f(const float val);

    /**
     * @brief: Initializes matrix with specified values (mxy; where x and y is the row and column number of specified value).
     *      __                      __
     *     | m00    m01    m02    m03 |
     *     | m10    m11    m12    m13 |
     *     | m20    m21    m22    m23 |
     *     |_m30    m31    m32    m33_|
     */
    Matrix4f(float m00, float m01, float m02, float m03,
            float m10, float m11, float m12, float m13,
            float m20, float m21, float m22, float m23,
            float m30, float m31, float m32, float m33);

    /**
     * @brief: Initializes matrix with values from reference to array of 16 elements.
     *      __                                           __
     *     |  vals[0]     vals[1]     vals[2]     vals[3]  |
     *     |  vals[4]     vals[5]     vals[6]     vals[7]  |
     *     |  vals[8]     vals[9]     vals[10]    vals[11] |
     *     |_ vals[12]    vals[13]    vals[14]    vals[15]_|
     */
    explicit Matrix4f(const float(&vals)[16]);

    virtual ~Matrix4f();

    /**
     * @brief: Returns transpose of the matrix.
     */
    Matrix4f transpose() const;

    /**
     * @brief: Returns determinant value of the matrix.
     */
    float det() const;

    /**
     * @brief: Returns inverse of the matrix.
     */
    Matrix4f inverse() const;

    /**
     * @brief: Returns value of matrix element at specified location.
     *
     * @param[in] i: Row number of the element to be accessed (0 based indexing).
     *
     * @param[in] j: Column number of the element to be accessed (0 based indexing).
     *
     * @returns: Value of the matrix element.
     */
    float at(int i, int j) const;

    /**
     * @brief: Sets value of matrix element at specified location.
     *
     * @param[in] i: Row number of the element to be updated (0 based indexing).
     *
     * @param[in] j: Column number of the element to be updated (0 based indexing).
     *
     * @param[in] newValue: Value to store at specified location.
     */
    void set(int i, int j, float newValue);

    /**
     * @brief: Returns matrix consist of elements resulted from element wise subtraction with supplied matrix element this matrix.
     *
     * @param[in] M: Matrix containing values to be subtracted from this matrix.
     *
     * @returns: Matrix resulted from element-wise subtraction.
     */
    Matrix4f operator-(const Matrix4f& M) const;

    /**
     * @brief: Returns matrix consist of elements resulted from element wise addition with supplied matrix element this matrix.
     *
     * @param[in] M: Matrix containing values to be added with this matrix.
     *
     * @returns: Matrix resulted from element-wise addition.
     */
    Matrix4f operator+(const Matrix4f& M) const;

    /**
     * @brief: Returns matrix consist of elements resulted multiplying each element with supplied value.
     *
     * @param[in] val: Scalar value to multiply with each element of the matrix.
     *
     * @returns: Matrix resulted from element-wise multiplication with scalar.
     */
    Matrix4f operator*(const float val) const;

    /**
     * @brief: Returns matrix consist of elements resulted from subtracting supplied values from each element of the matrix.
     *
     * @param[in] val: Scalar value to be subtracted from each element of the matrix.
     *
     * @returns: Matrix resulted from element-wise subtraction with scalar.
     */
    Matrix4f operator-(const float val) const;

    /**
     * @brief: Returns matrix consist of elements by adding supplied value with each element of the matrix.
     *
     * @param[in] val: Scalar value to be add with each element of the matrix.
     *
     * @returns: Matrix resulted from element-wise addition with scalar.
     */
    Matrix4f operator+(const float val) const;

    /**
     * @brief: Returns matrix consist of elements by dividing each element with suppled value.
     *
     * @param[in] val: Scalar value to be divided from each element of the matrix.
     *
     * @returns: Matrix resulted from element-wise division by scalar.
     */
    Matrix4f operator/(const float val) const;

    /**
     * @brief: Returns multiplication of two matrix = [this matrix] x [M].
     *
     * @param[in] M: Matrix to multiply with.
     *
     * @returns: Matrix resulted from multiplication.
     */
    Matrix4f operator*(const Matrix4f& M) const;

    /**
     * @brief: Return true if values of each element matches with elements of supplied matrix otherwise returns false.
     */
    bool operator==(const Matrix4f &matrix) const;

    /**
     * @brief: Assign values of the supplied matrix to this matrix.
     */
    Matrix4f& operator=(const Matrix4f& M);
};


/*
 * Specialized 4x4 Matrix to describe camera pose
 * Axis orientation = x: is +ve in right direction w.r.t camera.
 *                    y: is +ve in down direction w.r.t camera.
 *                    z: is +ve away from the camera in [facing towards world]
 * Axis scale: in meters
 * Stores 3 axis Rotation and translation
 *    Matrix is store in [R | T] in row major order
 *      __                         __
 *     | R0x0    R0x1    R0x2    Tx  |
 *     | R1x0    R1x1    R1x2    Ty  |
 *     | R2x0    R2x1    R2x2    Tz  |
 *     |_  0       0      0       1 _|
 *
 *     Where
 *
 *     Rotation Matrix
 *            __                   __
 *           | R0x0    R0x1    R0x2  |
 *     R  =  | R1x0    R1x1    R1x2  |
 *           |_R2x0    R2x1    R2x2 _|
 *
 *
 *	And
 *     Translation vector: stored in meters
 *            _    _       _                            _
 *           |  Tx  |     |  Translation in x in meters  |
 *     T  =  |  Ty  | =   |  Translation in y in meters  |
 *           |_ Tz _|     |_ Translation in z in meters _|
 *
 */
class PoseMatrix4f : public Matrix4f
{
public:
    /**
     * @brief: Default constructor initializes pose matrix with Identity rotation [3x3] and translation is set to origin [0, 0, 0]
     */
    PoseMatrix4f();

    /**
     * @brief: Copy constructor, to copy other pose matrix in to this.
     */
    PoseMatrix4f(const PoseMatrix4f &poseMat);

    /**
     * @brief: Initializes pose matrix with specified values of rotation and translation (in meters) elements.
     *      _                        _
     *     | r00    r01    r02    tx  |
     *     | r10    r11    r12    ty  |
     *     | r20    r21    r22    tz  |
     *     |_  0     0      0      1 _|
     */
    PoseMatrix4f(float r00, float r01, float r02, float tx,
            float r10, float r11, float r12, float ty,
            float r20, float r21, float r22, float tz);

    /**
     * @brief: Initializes pose matrix with values from specified reference to array of 12 elements.
     *      _                                         _
     *     | vals[0]    vals[1]    vals[2]    vals[3]  |
     *     | vals[4]    vals[5]    vals[6]    vals[7]  |
     *     | vals[8]    vals[9]    vals[10]   vals[11] |
     *     |_  0          0          0           1    _|
     */
    PoseMatrix4f(const float(&vals)[12]);

    /**
     * @brief: Initializes pose matrix with first 12 values from specified reference to array of 16 elements.
     *      _                                         _
     *     | vals[0]    vals[1]    vals[2]    vals[3]  |
     *     | vals[4]    vals[5]    vals[6]    vals[7]  |
     *     | vals[8]    vals[9]    vals[10]   vals[11] |
     *     |_  0          0          0           1    _|
     */
    PoseMatrix4f(const float(&vals)[16]);

    /**
     * @brief: Copy constructor for pose matrix.
     */
    PoseMatrix4f(const Matrix4f& poseMat);

    /**
     * @brief: Assignment operator from matrix.
     */
    PoseMatrix4f& operator=(const Matrix4f& poseMat);

    /**
     * @brief: Assignment operator from pose matrix.
     */
    PoseMatrix4f& operator=(const PoseMatrix4f& poseMat);
    ~PoseMatrix4f();

    /**
     * @brief: Returns inverse of the matrix.
     */
    PoseMatrix4f inverse() const;

    /**
     * @brief: Returns multiplication of two pose matrices. this pose matrix x poseMat2.
     *
     * @param[in] poseMat2M: Pose matrix to multiply with.
     *
     * @returns: Pose matrix resulted from multiplication.
     */
    PoseMatrix4f operator*(const PoseMatrix4f& poseMat2) const;

    /**
     * @brief: To verify if supplied reference to array of 12 values can form a pose matrix.
     *
     * @returns: True, if supplied values can for a pose matrix otherwise returns false.
     */
    static bool validate_pose_matrix(const float(&poseMatrix)[12]);

    /**
     * @brief: To verify validity of pose matrix.
     *
     * @returns: True the matrix is pose matrix otherwise returns false.
     */
    static bool validate_pose_matrix(const PoseMatrix4f& pose);
};
}
}

