;Note: that the square root, exp and log functions use a
; table as a floating point workspace.  As they are currently
; written, these two functions are not 'reentrant', which means
; they cannot be used in two separate tasks at the same time.



;Initialise variables used in the sqrt function.
TO math_init

;general:
  MAKE _wkspc Table (1 , 4)            ;used as a floating point workspace
  _mant_address := _wkspc . Pointer	   ;where to find the mantissa
  _exp_address := _mant_address + 3    ;where to find the exponent

;for sqrt:
  _root2 := 1.4142135

;for log:
 _l0 := 2.0000007
 _l1 := 6.6644078E-1
 _l2 := 4.1517740E-1
 _sqrto2 := 7.0710677E-1
 _log2 := 6.9314718E-1

;for exp:
_b6 :=	0.21702255e-3
_b5 :=	0.12439688e-2
_b4 :=	0.96788410e-2
_b3 :=	0.55483342e-1
_b2 :=	0.24022984
_b1 :=	0.69314698
_b0 :=	0.10000000e1
_log2e := 1.4426950
END


;The exponential function.
;Timing~30mS
TO exp (arg)
LOCAL xsq,ent,flag
  arg := arg as float
  IF arg = 0.0000000
    RETURN 1.0000000
  IF ABS arg > 21
  [ IF arg < 0.0000000
      RETURN 0.0000000
    THROW 88
  ]
  arg := arg * _log2e
  ent := ABS arg AS INT
  IF arg < 0.0000000
    ent := -ent-1
  xsq := arg - ent
  IF xsq < 5.0000000E-1
  [ xsq := xsq + 5.0000000E-1
    flag := 1
  ]
  ELSE flag := 0
  arg := (((((b6 * xsq + b5) * xsq + b4) * xsq + b3) * xsq + b2) * xsq + b1) *
    xsq + b0
  IF flag
    arg := arg * _sqrto2
  _wkspc . Element (0) := arg
  ? _exp_address := ? _exp_address + ent
  RETURN _wkspc . Element (0)
END



;The Natural Logarithm function.
;To get log10, multiply this by 1/log(10): [0.43429448]
;Time:~30 mS
TO log (arg)
LOCAL z,exp
  arg := arg as float
  IF arg <= 0.0000000
  [ IF arg = 0.0000000
    [ THROW 5
    ]
    THROW 88
  ]
  _wkspc . Element (0) := arg
  exp := (? _exp_address AND 63) - 32
  arg := (? ? ? _mant_address + $800000) / $1000000
  IF (arg < _sqrto2)
  [ 
    _wkspc . Element (0) := arg
    ? _exp_address := ? _exp_address + 1
    arg := _wkspc . Element (0)
    exp := exp - 1
  ]
  arg := (arg - 1) / (arg + 1)
  z := arg * arg
  arg := arg * ((_l2 * z + _l1) * z + _l0)
  RETURN exp * _log2 + arg
END





;The Square Root function.
;Timing:~30mS
TO sqrt (arg)
LOCAL x,y,exp
  arg := arg AS FLOAT
  IF arg <= 0.0
  [ IF arg = 0.0
      RETURN 0.0
    THROW 88
  ]
  _wkspc . Element (0) := arg
  exp := (? _exp_address AND 63) - 32
  x := (? ? ? _mant_address + $800000) / $1000000
  y := 5.7155001E-1 * (x + 7.5787002E-1)
  y := y + x / y
  IF exp AND 1
  [
    y := y * _root2
    _wkspc . Element (0) := y
    ? _exp_address := ? _exp_address + (exp - 1) DIV 2
  ]
  ELSE
  [
    _wkspc . Element (0) := y
    ? _exp_address := ? _exp_address + exp DIV 2
  ]
  x := _wkspc . Element (0)
  x := 2.5000000E-1 * x + arg / x
  RETURN x
END