#!perl -w
# (C) 2003 XDA Developers  itsme@xs4all.nl
#
# $Header$
#
#  vim: filetype=perl ts=4 sw=4 et vb t_vb=
use strict;
$|=1;
# this script demonstrates how to do fixed point calculations
# fixed point numbers are represented by the integer, and the nr of bits
# behind the comma. ( in the fraction )

dotestallarith();
#dotestrecip();
#dotestmsb();
#dotestlog();
exit(0);

# bit '15' or more will get you currently unhandled overflow errors
sub dotestallarith {
    my @a= (4, 8, 14);
    for my $i (0..@a*@a*@a-1) {
        print "-----testing $i\n";
        my $na= $a[$i % @a];
        my $nb= $a[($i/@a) % @a];
        my $nc= $a[($i/@a/@a) % @a];
        dotestarith($na, $nb, $nc);
    }
}

sub writeresult {
    my ($type, $real, $freal, $fixed)= @_;
    printf("    %-4s real:%s  freal:%s  fxp:%s   rerr=%d  ferr=%d\n", $type,
        tostr($real), tostr($freal), tostr($fixed), $real->{a}-$fixed->{a}, $freal->{a}-$fixed->{a});
}
sub dotestarith {
    my ($na, $nb, $nc)= @_;

    my $ra=3.14159265358979323844; my $a= tofxp($ra, $na); my $fa= toval($a);
    my $rb=2.71828182845904523536; my $b= tofxp($rb, $nb); my $fb= toval($b);

    #ra = real value
    #fa = fixed point converted back to real value ( losing precision doing so )
    #
    #fxXXX = fixedpoint result 
    #rlXXX = real result converted to fixedpoint
    #rfXXX = real result on truncated values converted to fixedpoint
    my $fxprod= fxpmul($a, $b, $nc);          my $rlprod= tofxp($ra*$rb, $nc); my $rfprod= tofxp($fa*$fb, $nc);       
    my $fxsum= fxpadd($a, $b, $nc);           my $rlsum= tofxp($ra+$rb, $nc);  my $rfsum= tofxp($fa+$fb, $nc);
    my $fxdiff= fxpsub($a, $b, $nc);          my $rldiff= tofxp($ra-$rb, $nc); my $rfdiff= tofxp($fa-$fb, $nc);
    my $fxquot= fxpdiv($a, $b, $nc);          my $rlquot= tofxp($ra/$rb, $nc); my $rfquot= tofxp($fa/$fb, $nc);
    my $fxndiv= fxpnewtdiv($a, $b, $nc);      my $rlndiv= tofxp($ra/$rb, $nc); my $rfndiv= tofxp($fa/$fb, $nc);
    my $fxmdiv= fxpmuldiv($a, $b, $nc);       my $rlmdiv= tofxp($ra/$rb, $nc); my $rfmdiv= tofxp($fa/$fb, $nc);
    my $fxloga= fxplog($a, $nc);              my $rlloga= tofxp(log($ra), $nc);my $rfloga= tofxp(log($fa), $nc);
    my $fxlogb= fxplog($b, $nc);              my $rllogb= tofxp(log($rb), $nc);my $rflogb= tofxp(log($fb), $nc);
    my $fxsqrta= fxpsqrt($a, $nc);            my $rlsqrta= tofxp(sqrt($ra), $nc);my $rfsqrta= tofxp(sqrt($fa), $nc);
    my $fxsqrtb= fxpsqrt($b, $nc);            my $rlsqrtb= tofxp(sqrt($rb), $nc);my $rfsqrtb= tofxp(sqrt($fb), $nc);
    my $fxexpta= fxpexp_t($a, $nc); 
    my $fxexptb= fxpexp_t($b, $nc); 
    my $fxexpa= fxpexp($a, $nc);            my $rlexpa= tofxp(exp($ra), $nc);my $rfexpa= tofxp(exp($fa), $nc);
    my $fxexpb= fxpexp($b, $nc);            my $rlexpb= tofxp(exp($rb), $nc);my $rfexpb= tofxp(exp($fb), $nc);
    my $fxpow= fxppow($a, $b, $nc);         my $rlpow= tofxp(pow($ra, $rb), $nc); my $rfpow= tofxp(pow($fa, $fb), $nc);

    printf("a=%s b=%s\n", tostr($a), tostr($b));
    writeresult("prod", $rlprod, $rfprod, $fxprod);
    writeresult("sum",  $rlsum, $rfsum, $fxsum);
    writeresult("diff", $rldiff, $rfdiff, $fxdiff);
    writeresult("quot", $rlquot, $rfquot, $fxquot);
    writeresult("mdiv", $rlmdiv, $rfmdiv, $fxmdiv);
    writeresult("ndiv", $rlndiv, $rfndiv, $fxndiv);
    writeresult("pow", $rlpow, $rfpow, $fxpow);
    writeresult("loga", $rlloga, $rfloga, $fxloga);
    writeresult("logb", $rllogb, $rflogb, $fxlogb);
    writeresult("sqta", $rlsqrta, $rfsqrta, $fxsqrta);
    writeresult("sqtb", $rlsqrtb, $rfsqrtb, $fxsqrtb);
    writeresult("expta", $rlexpa, $rfexpa, $fxexpta);
    writeresult("exptb", $rlexpb, $rfexpb, $fxexptb);
    writeresult("expa", $rlexpa, $rfexpa, $fxexpa);
    writeresult("expb", $rlexpb, $rfexpb, $fxexpb);
}

sub dotestrecip {
    for (my $fval= 0.1 ; $fval<10 ; $fval *= 1.3)
    {
        my $xval= tofxp($fval, 13);
        my $recip= newtreciprocal($xval, 13);
        printf("%s -> %s  1=%s\n",  tostr($xval), tostr($recip), tostr(fxpmul($xval, $recip, 13)));
    }
}

sub dotestmuldiv {
}
###################### special division algorithms ##########################################

#  dividend/divisor=quotient.
#  todo: write this in terms of int operations
sub fxpmuldiv {
    my ($a, $b, $n)= @_;
    my $msbit=GetMsb($b);
    my $shiftcount= $b->{n}-$msbit;

    my $normb= fxpshl($b, $shiftcount, $b->{n});
    my $norma= fxpshl($a, $shiftcount, $a->{n});

    for (1..6) {
        $b= fxpmul(fxpsub(tofxp(2,$n), $normb, $n), $b, $n);
        $a= fxpmul(fxpsub(tofxp(2,$n), $norma, $n), $a, $n);
    }

    return $a;
}

# returns  msb such that  2^msb <= $nr < 2*2^msb
sub GetMsb {
    my ($a)= @_;
    my $val= $a->{a};
    my $shifts= 0;
    while($val>1) {
        $val >>=1;
        $shifts++;
    }
    return $shifts;
}
sub dotestmsb {
    for (1..10) {
        printf("%s : msb=%d\n", tostr(tofxp($_,24)), GetMsb(tofxp($_,24)));
    }
}
=pod

1. Set pass counter, lp, for 6, enough for a 64-bit result. Check both
   operands for zero,
   If either is zero, go to step 10.
   Otherwise continue with step 2.
2. Find the most significant word of divisor, and see whether it is above
   or below the radix point,
   If it's below, normalization is to the left; go to step 3a.
   If it's above, normalization is to the right; go to step 3b.
   If it's right there, see whether it's already normalized.
   if so, skip to step 4.
   Otherwise, continue with step 3a.
3. a) Shift a copy of the most significant word of the divisor left until
      the MSB is one, counting the shifts as you go. Continue with step 4.
   b) Shift a copy of the most significant word of the divisor right until
      it is zero, counting the shifts as you go. Continue with step 4.
4.  Shift the actual divisor so that the MSB of the most significant word
   is one.
5. Shift the dividend right or left the same number of bits as calculated
   in step 3. This keeps the ratio between the dividend and the divisor the
   same.
6.  Offset = 1 - normalized divisor.
7.  Multiply the offset by the divisor, saving the result in a temporary
   register. Add the divisor to the temporary register to simulate the
   multiplication of 1 + offset by the divisor. Write the temporary
   register to the divisor.
8. Multiply the offset by the dividend, saving the result in a temporary
   simulate the multiplication of 1 + offset by the dividend. Write the
   temporary register to the divisor.
9. Decrement 1p,
   If it's not yet zero, go to step 6.
   Otherwise, the current dividend is the quotient; exit.
10. Overflow exit, leave with an error condition.

=cut
#  todo: write this in terms of int operations
sub fxpnewtdiv {
    my ($a, $b, $n)= @_;

    return fxpmul($a, newtreciprocal($b, $n), $n);
}
###################### calculate reciprocal using newton rapson #############################
# approximate reciprocal 'y' of 'x', by solving  x-1/y=0
# using newton-rapson.
#
#  f(y)= x-1/y   f'(y)= 1/y^2
#
#  y-f(y)/f'(y) = y-xy^2+y = y(2-x*y)
#
#  y[k+1] = y[k] - f(y[k]) / f'(y[k])
#
#   y1= y  *** this initial estimate is done by table lookup.
#   y2= y1*(2-x*y1)
#
#  todo: write this in terms of int operations
sub newtreciprocal {
    my ($x, $n)= @_;

    my $y= GetInitialEstimate($x);

    for (my $i= 0 ; $i < 10 ; $i++)
    {
        $y= fxpmul($y, fxpsub(tofxp(2, $n), fxpmul($x, $y, $n), $n), $n);
    }
    return $y;
}
# initial estimate for reciprocal calc: use highest bit.
#   ( if x=2^n + 2^(n-?) + ... -> return 2^-n
sub GetInitialEstimate {
    my ($x)= @_;

    my $val= toval($x);
    my $l2= int(log($val)/log(2));

    return tofxp(2**-$l2, $x->{n});
}

=pod

this describes a slightly different version of the above algorithm:

1. Set the loop counter,lp, for three passes. This is a reasonable number
   since the first estimate is 16-bits. Check the dividend and the divisor
   for zero.
   If no such error conditions exist, continue with step 2,
   Otherwise, go to step 10.
2. Find the most significant word of the divisor.
   Determine whether it is above or below the fixed-point radix point.
   In this case, the radix point is between the doublewords.
   Test to see if it is already normalized.
   If so, go to step 5.
3. Shift a copy of the most significant word of the divisor left or right
   until it is normalized, counting the shifts as you proceed.
4. Shift the actual divisor until its most significant one is the MSB of

   the third word of the divisor. This is to provide maximum precision for
   the operation.
5. Divide 10000000H by the most significant word of the divisor for a first
   approximation of the reciprocal. The greater the precision of this first
   estimate, the fewer passes will be required in the algorithm (the result
   of this division will be between one and two.)
   Shift the result of this division left one hex digit, four bits, to align
   it as an integer with a three-word fraction part. This initial estimate
   is stored in the divisor variable.
   Divide this first estimate by two to obtain the proportionality variable,
   proportion.
6. Perform a two's complement on proportion to simulate subtracting it from
   two. Multiply proportion by divisor. Leave the result in divisor.
7. Multiply proportion by the estimate, storing the results in both
   proportion and estimate. Decrement lp.
   If it isn't zero, continue with step 6.
   Otherwise, go to step 8.
8. Using the shift counter, shift, reposition divisor for the final
   multiplication.
9. Multiply divisor, now the reciprocal of the input argument, by the
   dividend to obtain the quotient. Write the proper number of words to the
   ouput and exit.
10. An attempt was made to divide zero or divide by zero; exit with error.

=cut
########################### arithmetic fixed point operations ###############################
#r(A,n)= A*2^-n
sub rint {
    my ($a)= @_;
    if ($a>0) {
        return int($a+0.5);
    }
    elsif ($a<0) {
        return int($a-0.5);
    }
    else {
        return 0;
    }

}
sub fxpdup {
    my ($a, $n)= @_;
    return {a=>shl($a->{a}, $n-$a->{n}), n=>$n};
}

sub tofxp {
    my ($a, $n)= @_;

    return {a=>rint($a*(1<<$n)), n=>$n};
}
sub toval {
    my ($a)= @_;

    return $a->{a}/(1<<$a->{n});
}
sub tostr {
    my ($a)= @_;

    return sprintf("%8.6f[%08lx:%2d]", toval($a), $a->{a}, $a->{n});
}

#r(C,nC)= r(A,nA) * r(B,nB)  -> C = A*B * 2^(nC-nA-nB)

sub fxpmul {
    my ($a, $b, $n) = @_;

    return {a=>shl64(intmul64($a->{a} , $b->{a}), $n-$a->{n}-$b->{n})->[1], n=>$n};
}

#r(C,nC)= r(A,nA) + r(B,nB)  -> C = ( A+B*2^(nA-nB) ) * 2^(nC-nA)
#                            or C = ( A*2^(nB-nA)+B ) * 2^(nC-nB)

sub fxpadd {
    my ($a, $b, $n) = @_;
    if ($b->{n} > $a->{n}) {
        return {a=>shl( intadd(shl($a->{a}, $b->{n}-$a->{n}) , $b->{a}), $n-$b->{n}), n=>$n};
    }
    else {
        return {a=>shl( intadd($a->{a} , shl($b->{a}, $a->{n}-$b->{n})), $n-$a->{n}), n=>$n};
    }
}
sub fxpsub {
    my ($a, $b, $n) = @_;
    if ($b->{n} > $a->{n}) {
        return {a=>shl( intsub(shl($a->{a}, $b->{n}-$a->{n}) , $b->{a}), $n-$b->{n}), n=>$n};
    }
    else {
        return {a=>shl(intsub($a->{a} , shl($b->{a}, $a->{n}-$b->{n})), $n-$a->{n}), n=>$n};
    }
}
#r(C,nC)= r(A,nA) / r(B,nB)  -> C = A / B * 2^(nB+nC-nA)
sub fxpdiv {
    my ($a, $b, $n)= @_;

    return {a=>intdiv64(shl64([0,$a->{a}], $n-$a->{n}+$b->{n}), $b->{a}), n=>$n};
}
sub fxpshl {
    my ($a, $shift, $n)= @_;
    return { a=>shl($a->{a}, $shift+$n-$a->{n}), n=>$n };
}
sub shl {
    my ($a, $n)= @_;
    return ($a<<$n) if ($n>0);
    return ($a>>-$n) if ($n<0);
    return $a;
}
sub shl64 {
    my ($a, $n)= @_;
    return [ ($a->[0]<<$n)|($a->[1]>>(32-$n)), ($a->[1]<<$n) ] if ($n>0);
    return [ ($a->[0]>>-$n), ($a->[0]<<32+$n)|($a->[1]>>-$n) ] if ($n<0);
    return [ $a->[0], $a->[1] ];
}

sub intadd {
    my ($a, $b)= @_;
    return int($a+$b);
}
sub intsub {
    my ($a, $b)= @_;
    return int($a-$b);
}
sub intmul64 {
    my ($a, $b)= @_;

    my $prod= $a*$b;
    return [ int($prod/2**32), int($prod) ];
}
sub intdiv64 {
    my ($a64, $b)= @_;
    return int(($a64->[0]*2**32 + $a64->[1])/$b);
}

#############################

# a(i) = 1/(1-2^-i) ,  i=1...
#
# r(L,nL)= ... r(A,nA)
#
# r(A,nA)= 2^scale * prod( a(i) )
#      where 'scale' is such that  2<= r(A,nA)*2^-scale < 1
#
# r(L,nL)= log(2^scale * prod( a(i) ) )
#        = scale*log(2) + sum( log(a(i)) )
#
# L*2^-nL = log(A*2^-nA) = log(A)-nA*log(2)
sub fxplog {
    my ($a, $n)= @_;

    if (fxtest($a)<=0) {
        return;
    }

    my $base= $n;
    # lg2 contains values [ 2*2^n .. 2^n >
    my @va2= map { rint(2**$base/(1-2**(-$_))) } (1..$base); unshift @va2, 0;
    my @lg2= map { -rint(2**$n*log(1-2**(-$_))) } (1..$base); unshift @lg2, 0;


    my $val= $a->{a};

    # scale $a such that it is just over 2*2^n : bit (n+1) = 1
    my $msb= GetMsb($a);
    $val = shl($val, 1+$base-$msb);
    my $l= -rint((1+$a->{n}-$msb)*log(2)*2**$n); #-$a->{n};

    for (1..$base)
    {
        while ($val>=$va2[$_])
        {
            $val -= ($val >> $_);
            $l += $lg2[$_];
        }
    }

    my $res= {a=>$l, n=>$n};
    return $res;
}
sub dotestlog {
    my $a= tofxp(3.1415926535, 24);
    printf("log(%s) = %s real=%s\n", tostr($a), tostr(fxplog($a, 24)), tostr(tofxp(log(toval($a)), 24)) );
    $a= tofxp(0.0001234, 24);                               
    printf("log(%s) = %s real=%s\n", tostr($a), tostr(fxplog($a, 24)), tostr(tofxp(log(toval($a)), 24)) );
    $a= tofxp(104, 24);                                     
    printf("log(%s) = %s real=%s\n", tostr($a), tostr(fxplog($a, 24)), tostr(tofxp(log(toval($a)), 24)) );
    $a= tofxp(1, 24);                                     
    printf("log(%s) = %s real=%s\n", tostr($a), tostr(fxplog($a, 24)), tostr(tofxp(log(toval($a)), 24)) );
}


# r(L,nL)= sqrt(r(A,nA))
#   todo: write this in terms of int operations
sub fxpsqrt {
    my ($a, $n)= @_;

    # a= (a+1)/2  - initial estimate
    my $sqrt= fxpshl(fxpadd($a, tofxp(1,$n), $n), -1, $n);

    for (0..7) {
        $sqrt= fxpshl(fxpadd($sqrt, fxpdiv($a, $sqrt, $n), $n), -1, $n);
    }

    return $sqrt;
}

# r(L,nL)= exp(r(A,nA))
#   todo: write this in terms of int operations
#     - currently way too inaccurate.
sub fxpexp_t {
    my ($a, $n)= @_;

    my $result= tofxp(1, $n);
    my $y= tofxp(1, $n);

    for (1..20) {
        $y= fxpmul($y, $a, $n);
        $y= fxpdiv($y,tofxp($_,$n), $n);
        $result= fxpadd($result, $y, $n);
    }

    return $result;
}

# r(A,nA) = A*2^-nA
# r(B,nB) = B*2^-nB = sum(2^(b(i)-nB)), where 0<=b(i)<32
#
# a^sum(2^(b(i)-nB)) = prod ( a^(2^(b(i)-nB)) )
sub fxppow {
    my ($a, $b, $n)= @_;

    my $res= tofxp(1, $n);
    my $aa= fxpdup($a, $n);

    # first multiply positive powers
    for (my $i= $b->{n} ; $i<32 ; $i++)
    {
        if ($b->{a}&(1<<$i))
        {
            $res= fxpmul($res, $aa, $n);
            print "positive power ", $i-$b->{n}, "\n";
        }
        $aa= fxpmul($aa, $aa, $n);
    }

    $aa= fxpsqrt($a, $n);

    # then multiply negative powers
    for (my $i= $b->{n}-1 ; $i>=0 ; $i--)
    {
        if ($b->{a}&(1<<$i))
        {
            $res= fxpmul($res, $aa, $n);
            print "negative power ", $i-$b->{n}, "\n";
        }
        $aa= fxpsqrt($aa, $n);
    }
    return $res;
}


sub pow {
    my ($a, $b)= @_;
    return $a**$b;
}
sub fxtest {
    my ($a)= @_;
    return $a->{a} <=> 0;
}
sub fxneg {
    my ($a)= @_;
    return tofxp(-$a->{a}, $a->{n});
}
sub fxpexp {
    my ($a, $n)= @_;
    if (fxtest($a)<0) {
        return fxpexp_neg(fxneg($a));
    }
    if (fxtest($a)>0) {
        return fxpexp_pos($a);
    }
    return tofxp(1, $n);
}
sub fxpexp_neg {
    my ($a, $n)= @_;
#  e^(-sum(a(i))*2^-n) = prod(e^(-a(i)*2^-n))


#  b(i) = 2^i
#  x= r(X,nX) = -sum(b(i), i in bits[x] )

#  exp(x)= prod(exp(-b(i)), i in bits[x] )

    my @exneg_values_big;
#  echo 'for (i=1 ; i<=5 ; i++) { print e(-(2^i)), ",\n" }' | bc -l

    my $exneg_values_small;
#  echo 'for (i=0 ; i<=32 ; i++) { print e(-(2^-i)), ",\n" }' | bc -l

    my $exp= tofxp(1, $n);

    my $mask= tofxp(2, $n);
    my $i=0;
    while ($a->{a}>=$mask->{a} && $i<@exneg_values_big) {
        if ($a->{a}&$mask->{a}) {
            $exp = fxpt_mul($exp, $exneg_values_big[$i]);
            $a->{a} &= ~$mask->{a};
        }
        $mask->{a}<<=1;
        $i++;
    }
    if ($mask->{a} && $a->{a}>=$mask->{a}) {
        return tofxp(999,$n); # overflow
    }

    $mask= tofxp(1,$n);
    $i=0;
    while ($a->{a} && $mask->{a} && $i<@exneg_values_small) {
        if ($a->{a}&$mask->{a}) {
            $exp = fxpt_mul($exp, $exneg_values_small[$i]);
            $a->{a} &= ~$mask->{a};
        }
        $mask->{a}>>=1;
        $i++;
    }
    return $exp;
}

sub fxpexp_pos {
    my ($a, $n)= @_;
#  e^(-sum(a(i))*2^-n) = prod(e^(-a(i)*2^-n))

#  b(i) = 2^i
#  x= r(X,nX) = -sum(b(i), i in bits[x] )

#  exp(x)= prod(exp(-b(i)), i in bits[x] )

    my @expos_values_big;
# echo 'for (i=1 ; i<=4 ; i++) { print e(2^i), ",\n" }' | bc -l

    my $expos_values_small;
# echo 'for (i=0 ; i<=32 ; i++) { print e(2^-i), ",\n" }' | bc -l

    my $exp= tofxp(1, $n);

    my $mask= tofxp(2, $n);
    my $i=0;
    while ($a->{a}>=$mask->{a} && $i<@expos_values_big) {
        if ($a->{a}&$mask->{a}) {
            $exp = fxpt_mul($exp, $expos_values_big[$i]);
            $a->{a} &= ~$mask->{a};
        }
        $mask->{a}<<=1;
        $i++;
    }
    if ($mask->{a} && $a->{a}>=$mask->{a}) {
        return tofxp(999,$n); # overflow
    }

    $mask= tofxp(1,$n);
    $i=0;
    while ($a->{a} && $mask->{a} && $i<@expos_values_small) {
        if ($a->{a}&$mask->{a}) {
            $exp = fxpt_mul($exp, $expos_values_small[$i]);
            $a->{a} &= ~$mask->{a};
        }
        $mask->{a}>>=1;
        $i++;
    }
    return $exp;
}