import Pkg; Pkg.add("Printf")

using Printf

function cordic(alpha, iteration = 24)
    # If the angle is greater than 2π, there may be a sign failure when defining a new sign.
    newsgn = isodd(Int(floor(alpha / 2π))) ? 1 : -1
    
    # Bringing the angle to the range [-π/2, π/2]
    alpha < -π/2 && return newsgn * cordic(alpha + π, iteration)
    alpha > π/2 && return newsgn * cordic(alpha - π, iteration)

    # A table of pre-calculated arctangents 2^(-j), where j = 0, 1, 2, ...
    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]

    # The table of scaling factors Kn, taken into account at the end
    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]

    # We select the value of the zoom level depending on the number of iterations.
    Kn = Kvalues[min(iteration, length(Kvalues))]  

    # The initial value of the vector is [cos(0), sin(0)] = [1, 0]
    v = [1, 0]  
    poweroftwo = 1  # the power of two, 2^0=1, varies with each iteration
    angle = angles[1]  # current rotation angle
    
    # The main iteration cycle of CORDIC
    for j = 0:iteration-1
        if alpha < 0
            sigma = -1  # turn counterclockwise
        else
            sigma = 1   # turn clockwise
        end
        
        factor = sigma * poweroftwo  # the coefficient for the rotation matrix

        # The rotation matrix for the angle atan(2^(-j)), written in CORDIC form
        R = [1 -factor
             factor  1]

        # Multiply the vector v by the matrix R (rotate the vector)
        v = R * v  

        # Updating the value of the remaining angle for the next iteration
        alpha -= sigma * angle 

        # Moving to the next power of two
        poweroftwo /= 2  

        # Updating the angle for the next iteration
        if j + 2 > length(angles)
            angle /= 2  # if the table is over, reduce the angle by half.
        else
            angle = angles[j + 2]  # otherwise, we take the following angle from the table
        end
    end

    # We apply a scaling factor to get the true values.
    v .*= Kn
    return v  # we return [cos(alpha), sin(alpha)]
end

function test_cordic()
    println("   x      sin(x)        Δsin         cos(x)        Δcos ")

    # Checking for angles from -90° to +90° in 15° increments
    for θ in -90:15:90 
        # Convert degrees to radians and run the CORDIC function
        cosθ, sinθ = cordic(deg2rad(θ))

        # We print the values in the format: angle, sin, sine error, cos, cosine error
        @printf("%+05.1f°  %+.8f (%+.8f) %+.8f (%+.8f)\n",
           θ, sinθ, sinθ - sind(θ), cosθ, cosθ - cosd(θ))
    end

    println("\nx(rad) sin(x) Δsin cos(x) Δcos ")

    # Checking for some angles in radians
    for θr in [-9, 0, 1.5, 6]
        cosθ, sinθ = cordic(θr)
        @printf("%+3.1f    %+.8f (%+.8f) %+.8f (%+.8f)\n",
           θr, sinθ, sinθ - sin(θr), cosθ, cosθ - cos(θr))
    end
end

test_cordic()