ground-programs/packages/math/math.grnd

fun -double !mod -double &in -double &modulus
    lesser $in $modulus &store
    not $store &store
    if $store %downloop

    lesser $in 0 &store
    if $store %uploop

    jump %end

    @downloop
    subtract $in $modulus &in
    lesser $in $modulus &store
    not $store &store
    if $store %downloop
    jump %end

    @uploop
    add $in $modulus &in
    lesser $in 0 &store
    if $store %uploop
    jump %end

    @end
    return $in
endfun

fun -double !pi
    return 3.141592653589793
endfun

fun -double !intexp -double &base -int &power
    lesser $power 1 &store
    set &ans 1
    if $store %end

    @loop
        subtract $power 1 &power
        multiply $ans $base &ans
        lesser $power 1 &store
        if $store %end
        jump %loop

    @end
    return $ans
endfun

fun -int !factorial -int &in
    set &idx $in
    set &ans 1
    lesser $idx 0 &store
    if $store %error

    @loop
        equal $idx 0 &store
        if $store %end
        multiply $ans $idx &ans
        subtract $idx 1 &idx
        jump %loop

    @end
    return $ans

    @error
    divide 1 0 &store
endfun

fun -double !cos -double &in
    # Get input modulo 2π
    call !pi &pi
    multiply $pi 2 &tau
    pusharg $in
    pusharg $tau
    call !mod &in
    
    set &idx 0
    set &ans 0

    # Check if $in < π
    greater $in $pi &store
    if $store %cos2
    jump %cos1

    @cos1
    # Taylor series at x=π/2
    multiply $idx 2 &2k+1
    add $2k+1 1 &2k+1
    
    # Get (x-π/2)^(2k+1)
    divide $pi 2 &store
    subtract $in $store &store
    pusharg $store
    tostring $2k+1 &2k+1
    stoi $2k+1 &2k+1
    pusharg $2k+1
    call !intexp &exp

    # Get (2k+1)!
    pusharg $2k+1
    call !factorial &fact

    # Divide
    divide $exp $fact &tempans

    # Add result to $ans
    tostring $idx &idx
    stod $idx &idx
    pusharg $idx
    stod "2" &store
    pusharg $store
    call !mod &store
    equal $store 0 &store
    if $store %subtract
    jump %add

    @add
    add $ans $tempans &ans
    jump %increment

    @subtract
    subtract $ans $tempans &ans
    jump %increment

    @increment    
    add $idx 1 &idx
    equal $idx 5 &store
    if $store %end
    jump %cos1

    @cos2
    # cos(x)=cos(2π-x)
    subtract $tau $in &in
    jump %cos1

    @end
    return $ans
endfun

fun -double !sin -double &in
    call !pi &halfpi
    divide $halfpi 2 &halfpi
    subtract $halfpi $in &in
    pusharg $in
    call !cos &store
    return $store
endfun

fun -double !tan -double &in
    pusharg $in
    call !sin &store
    pusharg $in
    call !cos &store2
    divide $store $store2 &store
    return $store
endfun

fun -double !random -int &seed
    set &m 2147483648
    set &a 1103515245
    set &c 12345
    multiply $seed $a &store
    add $store $c &store
    tostring $store &store
    stod $store &store
    pusharg $store
    pusharg $m
    call !mod &store
    divide $store 2147483648 &store
    return $store
endfun

fun -double !itod -int &int
    tostring $int &int
    stod $int &int
    return $int
endfun

fun -int !dtoi -double &double
    tostring $double &double
    stoi $double &double
    return $double
endfun

fun -double !sqrt -double &in
    # Finding sqrt(a) is the same as finding the roots of
    # f(x) = x^2 - a
    # Using Newton's method: x_(k+1) = x_k - f(x_k)/f'(x_k) = x_k - ((x_k)^2+a)/2(x_k)
    @loop
        multiply $ans $ans &num
        add $ans $in &num

        multiply $ans 2 &dom

        divide $num $dom &store
        equal $store 0 &store2
        if $store2 %end

        subtract $ans $store &ans
        jump %loop

    @end
    return $ans
endfun