--
--  Uses Newton-Raphson to get Y = A**(-1/N):
--    Inverse_Nth_Root =  Y_k+1 = Y_k + (1 - A * Y_k**N) * Y_k / N.
--  where  N is 2.
--
generic 

  type Real is digits <>;
  --  Anything up to Real'Digits = 24 digits is OK.  This is not checked.

package invsqrt is
     
  function Inverse_Sqrt (Arg : Real) return Real;
  pragma Inline(Inverse_Sqrt);

end invsqrt;
package body invsqrt is

  function Inverse4_Sqrt_At (X : Real) return Real;
--pragma Inline (Inverse4_Sqrt_At);

  ---------------------
  -- Inverse_Sqrt_At --
  ---------------------
  --
  -- Fit quartic spline to 2 pts. on Inverse_Sqrt curve.
  -- Should be good to 10+ bits on [1,4]. IF  No_of_Iterations := 3, THEN
  -- GOOD ENOUGH FOR  Real'Digits = 24.
  --   
  function Inverse4_Sqrt_At (X : Real) return Real is
     X3, X2 : Real;
   --X4, X3, X2 : Real;
     Y : Real;
     a1 : constant Real :=  2.05658516425871E+00;
     b1 : constant Real := -1.89528168315569E+00;
     c1 : constant Real :=  1.16136225073854E+00;
     d1 : constant Real := -3.70446006083701E-01;
     e1 : constant Real :=  4.74502518096651E-02;

     a2 : constant Real :=  1.45422531573500E+00;
     b2 : constant Real := -6.70083265209039E-01;
     c2 : constant Real :=  2.05301780727833E-01;
     d2 : constant Real := -3.27431103706594E-02;
     e2 : constant Real :=  2.09702467647666E-03;

  begin 

    X2 := X*X; 
    X3 := X2*X; 
    --X4 := X2*X2; 

    IF X > 2.0 THEN  

    -- Y   := a2 + X*(b2 + X*(c2 + X*(d2 + X*e2)));
    -- Y   := a2 + b2*X + c2*X2 + d2*X3 + e2*X4;  -- fastest on dec alpha
       Y   := a2 + b2*X + c2*X2 + (d2 + e2*X)*X3 ;

    ELSE

    -- Y   := a1 + X*(b1 + X*(c1 + X*(d1 + X*e1)));
    -- Y   := a1 + b1*X + c1*X2 + d1*X3 + e1*X4;
       Y   := a1 + b1*X + c1*X2 + (d1 + e1*X)*X3;

    END IF;

    return Y;
 
  end Inverse4_Sqrt_At;

  ---------------------
  -- Inverse_Sqrt_At --
  ---------------------
  --
  -- Fit cubic spline to 2 pts.
  -- Should be good to 7+ bits on [1,4].  IF  No_of_Iterations := 3, THEN
  -- ALMOST GOOD ENOUGH FOR  Real'Digits = 18, SO USE IF Real'Digits = 15.
  --   
  function Inverse3_Sqrt_At (X : Real) return Real is
     X3, X2 : Real;
     Y : Real;
     a1 : constant Real :=  1.84286198063662E+00;  
     b1 : constant Real := -1.29816848576085E+00;
     c1 : constant Real :=  5.43101858353914E-01;
     d1 : constant Real := -8.92682549640998E-02;

     a2 : constant Real :=  1.30310020329903E+00;
     b2 : constant Real := -4.58971869702085E-01;
     c2 : constant Real :=  9.60077517292674E-02;
     d2 : constant Real := -7.89027355372562E-03;

  begin 

    X2 := X*X; 
    X3 := X2*X; 

    IF X > 2.0 THEN  

     Y   := a2 + b2*X + c2*X2 + d2*X3;

    ELSE

     Y   := a1 + b1*X + c1*X2 + d1*X3;

    END IF;

    return Y;
 
  end Inverse3_Sqrt_At;

  pragma Inline (Inverse3_Sqrt_At);

  ------------------
  -- Inverse_Sqrt --
  ------------------
  --
  --  Uses Newton-Raphson to get Y = A**(-1/N):
  --    Inverse_Nth_Root =  Y_k+1 = Y_k + (1 - A * Y_k**N) * Y_k / N.
  --
  --     N : constant Integer := 2;
  --
  --IF Real'Digits < 25 THEN  
  --   No_of_Iterations := 3; -- Should be good to >80 bits.
  --ELSE
  --   No_of_Iterations := 4; --  Should be good to >160 bits.
  --END IF;
  --   
  function Inverse_Sqrt (Arg : Real) return Real is
   
     Scaled_Arg_Over_N  : Real;
     Scaled_Arg   : Real := Arg;
     Scale_Factor : Real := 1.0;
     Y_0          : Real;
     N            : constant Integer := 2;
     One_Over_N   : constant Real := 0.5;
     One_Plus_One_Over_N  : constant Real := 1.0 + One_Over_N;

     No_of_Iterations  : constant Integer := 3;

  begin 
   
   IF Arg  <= 0.0 THEN
      raise Constraint_error;
   END IF;
   
   IF Arg > 1.0 THEN

   IF Scaled_Arg > 256.0 THEN
      Scaled_Arg   := Scaled_Arg * 0.0625**2; 
      Scale_Factor := Scale_Factor * 0.0625;
      while Scaled_Arg > 256.0 loop
         Scaled_Arg   := Scaled_Arg * 0.0625**2; 
         Scale_Factor := Scale_Factor * 0.0625;
      end loop;
   END IF;

   IF Scaled_Arg > 16.0 THEN
      Scaled_Arg   := Scaled_Arg * 0.0625; 
      Scale_Factor := Scale_Factor * 0.25;
   END IF;

   IF Scaled_Arg > 4.0 THEN
      Scaled_Arg   := Scaled_Arg * 0.25; 
      Scale_Factor := Scale_Factor * 0.5;
   END IF;

   ELSE

   IF Scaled_Arg < 0.0625**2 THEN
      Scaled_Arg   := Scaled_Arg * 256.0; 
      Scale_Factor := Scale_Factor * 16.0;
      while Scaled_Arg < 0.0625**2 loop
         Scaled_Arg   := Scaled_Arg * 256.0; 
         Scale_Factor := Scale_Factor * 16.0;
      end loop;
   END IF;

   IF Scaled_Arg < 0.0625 THEN
      Scaled_Arg   := Scaled_Arg * 16.0; 
      Scale_Factor := Scale_Factor * 4.0;
   END IF;

   --IF Scaled_Arg < 0.25 THEN
   --   Scaled_Arg   := Scaled_Arg * 4.0; 
   --   Scale_Factor := Scale_Factor * 2.0;
   --END IF;

   IF Scaled_Arg < 0.25 THEN
      Scaled_Arg   := Scaled_Arg * 16.0; 
      Scale_Factor := Scale_Factor * 4.0;
   ELSE
      Scaled_Arg   := Scaled_Arg * 4.0; 
      Scale_Factor := Scale_Factor * 2.0;
   END IF;

   END IF;
  
     Scaled_Arg_Over_N := One_Over_N * Scaled_Arg; 

     Y_0 := Inverse4_Sqrt_At (Scaled_Arg); 
 
     for j in 1 .. No_of_Iterations-1 loop
        Y_0 := Y_0 * (One_Plus_One_Over_N - Scaled_Arg_Over_N * Y_0*Y_0); 
     end loop;
     
     -- last iteration (better accuracy, esp. at large N):
     Y_0 := Y_0 + Y_0 * (One_Over_N - Scaled_Arg_Over_N * Y_0*Y_0);

     return Y_0 * Scale_Factor;
      
  end Inverse_Sqrt;

end invsqrt;
with Text_IO;  use Text_IO;
with invsqrt; 
procedure sqr1tst1 is

 type Real is digits 15;
 
 Max : Real := 0.0;
 X, Y : Real := 0.0;

 Package FLIO is new Float_IO (Real);
 use FLIO;

 Package invsqr is new invsqrt (Real);
 use invsqr;

begin

  X :=  1.00000000;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    Y := 1.0 - Inverse_SQRT(X*X) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
    X := X + 2.0*1024.0**2;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.000000001;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 1000.1;
    Y := 1.0 - Inverse_SQRT(X*X) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.000000001;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 1.1;
    Y := 1.0 - Inverse_SQRT(X*X) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.000000001;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.001;
    Y := 1.0 - Inverse_SQRT(X*X) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.000000001;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.00001;
    Y := 1.0 - Inverse_SQRT(X*X) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.1;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.0000001;
    Y := 1.0 - Inverse_SQRT(X*X) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;

  X :=  0.0000000001111;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.01111;
    Y := 1.0 - Inverse_SQRT(X**2) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.0000000001111;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.0001111;
    Y := 1.0 - Inverse_SQRT(X**2) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.01;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.000001111;
    Y := 1.0 - Inverse_SQRT(X**2) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
  X :=  0.0;

  Max := -1.0E20;
  for I in 1..10000000 loop
  
    X := X + 0.00000000000000111;
    Y := 1.0 - Inverse_SQRT(X**2) * X;
    IF Abs Y > Max THEN    
       Max := Abs Y;
    END IF;
  end loop;
  
  new_line; put("Max error is:  "); put(Max); New_line;
  
end;
with Ada.Numerics.Generic_Elementary_Functions;
with Text_IO;  use Text_IO;
with invsqrt; 
procedure sqr1tst2 is

 type Integer32 is range -2**31..2**31-1;
 
 type Real is digits 15;
 
 X, Y : Real := 0.0;

 package Mathz is new Ada.Numerics.Generic_Elementary_Functions(Real);
 use Mathz;

 Package FLIO is new Float_IO (Real);
 use FLIO;

 Package invsqr is new invsqrt (Real);
 use invsqr;

begin

  X :=  0.1;
  Y :=  0.0;

  for I in 1..100000000 loop
  
    X := X + 1.0e-7;
    Y := Y + Inverse_SQRT(X);
   -- Y := Y + 1.0 / SQRT(X);

  end loop;
  
  new_line;  put(Y); New_line;
  
end;