cordic

import numpy as np

pi = np.pi

def cordic(beta:float, n:int)-> list[float]:
    """
    This function computes [cos(beta), sin(beta)] using n iterations. Increasing n will increase the precision.

    Args:
        beta (float): the angle in radians
        n (integer): the number of iterations. Maximum number of n is 23 due to the table sizes.

    Returns:
        result (list): a list with the cos(beta) as the first argument and sin(beta) as the second argument
    """

    if beta < -pi/2 or beta > pi/2:
        if beta < 0:
            result = cordic(beta + pi, n)
        else:
            result = cordic(beta - pi, n)
        result = -result
        return result

    # Initialization of tables of constants used by CORDIC
    # need a table of arctangents of negative powers of two, in radians:
    # angles = atan(2.^-(0:27));
    angles =  [
        0.78539816339745, 0.46364760900081, 0.24497866312686, 0.12435499454676,
        0.06241880999596, 0.03123983343027, 0.01562372862048, 0.00781234106010,
        0.00390623013197, 0.00195312251648, 0.00097656218956, 0.00048828121119,
        0.00024414062015, 0.00012207031189, 0.00006103515617, 0.00003051757812,
        0.00001525878906, 0.00000762939453, 0.00000381469727, 0.00000190734863,
        0.00000095367432, 0.00000047683716, 0.00000023841858, 0.00000011920929,
        0.00000005960464, 0.00000002980232, 0.00000001490116, 0.00000000745058 ]

    # and a table of products of reciprocal lengths of vectors [1, 2^-2j]:
    # Kvalues = cumprod(1./abs(1 + 1j*2.^(-(0:23))))
    Kvalues = [
        0.70710678118655, 0.63245553203368, 0.61357199107790, 0.60883391251775,
        0.60764825625617, 0.60735177014130, 0.60727764409353, 0.60725911229889,
        0.60725447933256, 0.60725332108988, 0.60725303152913, 0.60725295913894,
        0.60725294104140, 0.60725293651701, 0.60725293538591, 0.60725293510314,
        0.60725293503245, 0.60725293501477, 0.60725293501035, 0.60725293500925,
        0.60725293500897, 0.60725293500890, 0.60725293500889, 0.60725293500888 ]

    Kn = Kvalues[min(n, len(Kvalues))]

    # Initialize loop variables:
    v = [1,0] # start with 2-vector cosine 1 and sine of zero
    power_of_two = 1
    angle = angles[0]

    # Iterations
    for j in range(0,n-1):
        if beta < 0:
            sigma = -1
        else:
            sigma = 1

        factor = sigma * power_of_two
        # Note the matrix multiplication can be done using scaling by powers of two and addition subtraction
        R = [[1, -factor],[factor, 1]]
        v = np.dot(R, v)
        beta = beta - sigma * angle # update the remaining angle
        power_of_two = power_of_two / 2
        # update the angle from table, or eventually by just dividing by two
        if j+2 > len(angles):
            angle = angle / 2
        else:
            angle = angles[j+1]

    # Adjust length of output vector to be [cos(beta), sin(beta)]
    v = v * Kn
    result = v
    return result


def main():
    zero = cordic(0, 23)      # expected output: [1, 0]
    print(zero)
    pitwo = cordic(pi/2,23)    # expected output: [0, 1]
    print(pitwo)
    pialone = cordic(pi, 23)      # expected output: [-1, 0]
    print(pialone)
    pithree = cordic(pi/3, 23)   # expected output: [0.5, sqrt(3)/2]
    print(pithree[0], pithree[1])
    pifour = cordic(pi/4, 23)   # expected output: [sqrt(2)/2, sqrt(2)/2]
    print(pifour[0],pifour[1])
    pisix = cordic(pi/6, 23)   # expected output: [sqrt(3)/2, 0.5]
    print(pisix[0],pisix[1])
    pi32 = cordic(3*pi/2, 23) # expected output: [0, -1]
    print(pi32[0], pi32[1])

if __name__ == "__main__":
    main()