import numpy as np
import math

#convert row vector to column vector
def row_vec2col_vec(row_vec):
    vec_length = len(row_vec)
    col_vec = np.zeros((vec_length, 1))

    for idx in range(vec_length):
        col_vec[idx, 0] = row_vec[idx]

    return col_vec


#convert colum vector to row vector
def col_vec2row_vec(col_vec):
    vec_length = len(col_vec)
    row_vec = np.zeros((1, vec_length))

    for idx in range(vec_length):
        row_vec[0, idx] = col_vec[idx]

    return row_vec


#Transpose matrix
def matrix_transpose(input_matrix):
    """Not to be used on vectors"""
    dim2 = len(input_matrix[0, :])
    dim1 = len(input_matrix[:, 0])

    matrix_result = np.zeros((dim2, dim1))
    for idx1 in range(dim2):
        for idx2 in range(dim1):
            matrix_result[idx1, idx2] = input_matrix[idx2, idx1]

    return matrix_result

#Form the skew-symmetric (cross-product) matrix from a given vector
def skew_sym(vector_input):
    skew_sym_matrix = np.zeros((3, 3))
    skew_sym_matrix[0, 1] = (-1.0) * vector_input[2]
    skew_sym_matrix[0, 2] = vector_input[1]
    skew_sym_matrix[1, 0] = vector_input[2]
    skew_sym_matrix[1, 2] = (-1.0) * vector_input[0]
    skew_sym_matrix[2, 0] = (-1.0) * vector_input[1]
    skew_sym_matrix[2, 1] = vector_input[0]

    return skew_sym_matrix


#Compute the one-norm of a matrix
def one_norm(matrix_input):
    nRows = len(matrix_input[:, 0])
    nCols = len(matrix_input[0, :])

    sum_matrix = np.zeros((1, nCols))
    for idx in range(nCols):
        sum_matrix[0, idx] = np.sum(matrix_input[:, idx])

    result = np.max(sum_matrix)

    return result


#Compute the infinity norm of a matrix
def inf_norm(matrix_input):
    nRows = len(matrix_input[:, 0])
    nCols = len(matrix_input[0, :])

    sum_matrix = np.zeros((nRows, 1))
    for idx in range(nRows):
        sum_matrix[idx, 0] = np.sum(matrix_input[idx, :])

    result = np.max(sum_matrix)

    return result


#Compute the 2-norm
def two_norm(matrix_input):
    numEl = len(matrix_input)

    sumVal = 0
    for idx in range(numEl):
        sumVal = sumVal + (matrix_input[idx] ** 2)

    result = math.sqrt(sumVal)

    return result



#Determine "s" and "m" necessary for computing the matrix exponential
def m_s(matrix_input):
    theta_m = np.zeros((5, 1))
    theta_m[0, 0] = 1.495585217958292e-2 #theta3
    theta_m[1, 0] = 2.539398330063230e-1 #theta5
    theta_m[2, 0] = 9.504178996162932e-1 #theta7
    theta_m[3, 0] = 2.097847961257068 #theta9
    theta_m[4, 0] = 4.25 #theta13

    m_s = np.zeros((2, 1))

    d4 = np.sqrt(np.sqrt(one_norm(matrix_input ** 4)))
    d6 = np.sqrt(d4)
    eta1 = np.max(np.array([d4, d6]))
    if eta1 <= theta_m[0, 0]:
        m_s[0, 0] = 3
        m_s[1, 0] = 0.0

        return m_s

    eta2 = np.max(np.array([d4, d6]))
    if eta2 <= theta_m[1, 0]:
        m_s[0, 0] = 5
        m_s[1, 0] = 0.0

        return m_s

    d8 = np.sqrt(d6)
    eta3 = np.max(np.array([d6, d8]))
    if eta3 <= theta_m[2, 0]:
        m_s[0, 0] = 7
        m_s[1, 0] = 0

        return m_s

    elif eta3  <= theta_m[3, 0]:
        m_s[0, 0] = 9
        m_s[1, 0] = 0.0

        return m_s

    d10 = np.sqrt(d8)
    eta4 = np.max(np.array([d8, d10]))
    eta5 = np.min(np.array([eta3, eta4]))

    m_s[0, 0] = 13
    m_s[1, 0] = (1.0 / np.log(2)) * np.log((theta_m[4, 0]) / eta5)

    return m_s


#Compute the matrix exponential
def expm(matrix_input):
    nRows = len(matrix_input[:, 0])
    nCols = len(matrix_input[0, :])

    m_s_val = m_s(matrix_input)
    s_val = m_s_val[1, 0]
    f_val = 0.8

    A1 = (1.0 / (2 ** s_val)) * matrix_input
    X = A1
    c_val = 0.5;
    big_E = np.eye(nRows) + (c_val * A1)
    big_D = np.eye(nRows) - (c_val * A1)
    q = 6
    p = True

    for idx in range(2, (q + 1)):
        c_val = c_val * (q - idx + 1) / (idx * ((2 * q) - idx + 1))
        X = np.dot(A1, X)
        cX = c_val * X
        big_E = big_E + cX

        if p:
            big_D = big_D + cX
        else:
            big_D = big_D - cX

        p = not(p)

    big_E = np.dot(np.linalg.inv(big_D), big_E)

    for idx in range(int(s_val)):
        big_E = np.dot(big_E, big_E)

    return big_E


#Array indices to linear index
def arr2idx(column_number, row_number, num_rows):
    linIdx = (column_number * num_rows) + row_number

    return linIdx

#Select element of one-dimensional column-wise matrix
def select_element(matrix_input, row_num, col_num, num_rows):
    linIdx = arr2idx(col_num, row_num, num_rows)

    result = matrix_input[linIdx]

    return result

#Select slice of one-dimensional column-wise matrix
def select_array(matrix_input, row_range, col_range, num_rows):
    begin_row = row_range[0]
    end_row = row_range[1]
    begin_col = col_range[0]
    end_col = col_range[1]

    num_new_rows = end_row - begin_row + 1
    num_new_cols = end_col - begin_col + 1
    num_elements = num_new_rows * num_new_cols
    result = np.zeros(num_elements)

    element_counter = 0
    for col_idx in range(begin_col, (end_col + 1)):
        for row_idx in range(begin_row, (end_row + 1)):
            lin_idx = arr2idx(col_idx, row_idx, num_rows)
            result[element_counter] = matrix_input[lin_idx]
            element_counter = element_counter + 1

    return result

#Matrix multiplication
def mat_mul(matA, matB):
    matA_size = matA.shape
    matB_size = matB.shape

    first_dim = matA_size[0]
    second_dim = matA_size[1]
    third_dim = matB_size[1]

    result = np.zeros((first_dim, third_dim))

    for i in range(first_dim):
        for j in range(third_dim):
            scalarInterResult = 0

            for k in range(second_dim):
                firstComponent = matA[i, k]
                secondComponent = matB[k, j]
                scalarInterResult = scalarInterResult + (firstComponent * secondComponent)

            result[i, j] = scalarInterResult

    return result