with Ada.Numerics.Elementary_Functions;

package body Dgeqrf_Pkg with SPARK_Mode => On is

   use Ada.Numerics.Elementary_Functions;

   function Get_Elem
     (A : Matrix_Real_Type; I, J : Integer) return Float
   is (Float_Vectors.Element
        (A.Data, Natural ((I - 1) + (J - 1) * A.Leading_Dimension)));

   procedure Set_Elem
     (A : in out Matrix_Real_Type; I, J : Integer; Val : Float) is
   begin
      Float_Vectors.Replace_Element
        (A.Data, Natural ((I - 1) + (J - 1) * A.Leading_Dimension), Val);
   end Set_Elem;

   function Sign_Of (X : Float) return Float is
     (if X < 0.0 then -1.0 else 1.0);

   procedure Compute_Qr_Factorization
     (A      : in out Matrix_Real_Type;
      Tau    : in out Vector_Real_Type;
      Work   : in out Vector_Real_Type;
      M_Rows : in     Integer;
      N_Cols : in     Integer;
      LDA    : in     Integer;
      LWork  : in     Integer;
      Status :    out QR_Status_Type)
   is
      K_Min : constant Integer := Integer'Min (M_Rows, N_Cols);
   begin

      if M_Rows < 0 then Status := ILLEGAL_ARGUMENT_M; return; end if;
      if N_Cols < 0 then Status := ILLEGAL_ARGUMENT_N; return; end if;
      if LDA < Integer'Max (1, M_Rows) then
         Status := ILLEGAL_ARGUMENT_LDA; return;
      end if;
      if LWork /= -1
        and then not (K_Min = 0 and then LWork >= 1)
        and then not (M_Rows >= 1 and then LWork >= N_Cols)
      then
         Status := ILLEGAL_ARGUMENT_LWORK; return;
      end if;

      if LWork = -1 then Status := SUCCESS; return; end if;

      if K_Min = 0 then Status := SUCCESS; return; end if;

      A.Rows              := M_Rows;
      A.Columns           := N_Cols;
      A.Leading_Dimension := LDA;

      while Natural (Float_Vectors.Length (Tau.Data)) < K_Min loop
         Float_Vectors.Append (Tau.Data, 0.0);
         pragma Loop_Invariant
           (Natural (Float_Vectors.Length (Tau.Data)) <= K_Min);
      end loop;
      Tau.Item_Count := K_Min;

      for K in 1 .. K_Min loop
         declare
            Alpha    : constant Float := Get_Elem (A, K, K);
            Xnorm_Sq : Float := 0.0;
            Beta     : Float;
            Tau_K    : Float;
            Scale    : Float;
            W        : Float;
            X_I      : Float;
         begin
            for I in K + 1 .. M_Rows loop
               X_I := Get_Elem (A, I, K);
               Xnorm_Sq := Xnorm_Sq + X_I * X_I;
               pragma Loop_Invariant (Xnorm_Sq >= 0.0);
            end loop;

            if Xnorm_Sq = 0.0 and then Alpha >= 0.0 then
               Float_Vectors.Replace_Element
                 (Tau.Data, Natural (K - 1), 0.0);
            else
               Beta  := -Sign_Of (Alpha) * Sqrt (Alpha * Alpha + Xnorm_Sq);
               if Alpha - Beta = 0.0 then
                  Float_Vectors.Replace_Element
                    (Tau.Data, Natural (K - 1), 0.0);
               else
                  Tau_K := (Beta - Alpha) / Beta;
                  Scale := 1.0 / (Alpha - Beta);

                  for I in K + 1 .. M_Rows loop
                     Set_Elem (A, I, K, Get_Elem (A, I, K) * Scale);
                  end loop;

                  Set_Elem (A, K, K, Beta);

                  Float_Vectors.Replace_Element
                    (Tau.Data, Natural (K - 1), Tau_K);

                  for J in K + 1 .. N_Cols loop
                     W := Get_Elem (A, K, J);
                     for I in K + 1 .. M_Rows loop
                        W := W + Get_Elem (A, I, K) * Get_Elem (A, I, J);
                     end loop;

                     Set_Elem
                       (A, K, J, Get_Elem (A, K, J) - Tau_K * W);

                     for I in K + 1 .. M_Rows loop
                        Set_Elem
                          (A, I, J,
                           Get_Elem (A, I, J)
                           - Tau_K * W * Get_Elem (A, I, K));
                     end loop;
                  end loop;
               end if;
            end if;
         end;
         pragma Loop_Invariant (Tau.Item_Count = K_Min);
         pragma Loop_Invariant
           (Natural (Float_Vectors.Length (Tau.Data)) >= K_Min);
         pragma Loop_Invariant (A.Rows = M_Rows);
         pragma Loop_Invariant (A.Columns = N_Cols);
         pragma Loop_Invariant (A.Leading_Dimension = LDA);
      end loop;

      if Float_Vectors.Length (Work.Data) = 0 then
         Float_Vectors.Append (Work.Data, Float (Integer'Max (1, N_Cols)));
      else
         Float_Vectors.Replace_Element
           (Work.Data, 0, Float (Integer'Max (1, N_Cols)));
      end if;
      Work.Item_Count := Integer'Max (1, N_Cols);

      Status := SUCCESS;
   end Compute_Qr_Factorization;

end Dgeqrf_Pkg;