with
  unchecked_deallocation,
  text_io,
  ada.numerics.long_elementary_functions,
  terms;

package body nlp is

  function norm (n : in integer;
                 v : in terms.fmat1_ptr) return long_float is

    normv : long_float;

  begin
    normv := 0.0;

    for k in 1 .. n loop
      if abs v(k) > normv then
        normv := abs v(k);
      end if;
    end loop;

    return normv;
  end norm;

  package body qnewton is

    subtype vec is terms.fmat1;

    subtype vec_ptr is terms.fmat1_ptr;

    pragma controlled(vec_ptr);

    procedure free_vec is new unchecked_deallocation(vec,vec_ptr);

    procedure linesearch (n,maxls : in integer;
                          lstol   : in long_float;
                          f       : in out long_float;
                          x,g     : in terms.fmat1_ptr;
                          d       : in terms.fmat1_ptr) is

      ls                   : integer;
      a,b,fa,fb,dfa,dfb,df : long_float;
      maxdf,alpha          : long_float;
      x0                   : vec_ptr;

      function cubic (a,b,fa,fb,dfa,dfb : in long_float) return long_float is

        use
          ada.numerics.long_elementary_functions;

        u1,r,s,sr,rs,u2,du,x : long_float;

      begin
        -- check limits of interval
        if dfa = 0.0 then
          return a;
        end if;
        if dfb = 0.0 then
          return b;
        end if;

        -- check inside interval
        u1 := 3.0 * (fa - fb) / (b - a) + dfa + dfb;
        r := abs u1;
        s := sqrt(abs dfa) * sqrt(abs dfb);
        sr := (s / r) ** 2;
        rs := (r / s) ** 2;

        -- calculate u2
        if (dfa < 0.0 and dfb > 0.0) or (dfa > 0.0 and dfb < 0.0) then
          -- dfa * dfb < 0.0
          if r >= s then
            u2 := sqrt(1.0 + sr) * r;
          else
            u2 := sqrt(1.0 + rs) * s;
          end if;
        else
          -- dfa * dfb > 0.0
          if r >= s then
             u2 := sqrt(1.0 - sr) * r;
          else
             -- u2 is not a real number
             return (a + b) * 0.5;
          end if;
        end if;

        -- calculate du
        if u1 > 0.0 then
          du := -dfa * dfb / (u2 + u1);
        else
          du := u2 - u1;
        end if;

        -- cubic interpolation
        x := b - (dfb + du) / (dfb - dfa + u2 + u2) * (b - a);
        if a <= x and x <= b then
          return x;
        else
          return (a + b) * 0.5;
        end if;
      end cubic;

    begin
      -- memory allocation
      x0 := new vec (1 .. n);

      -- x0
      for k in 1 .. n loop
        x0(k) := x(k);
      end loop;

      -- initiate fa
      fa := f;

      -- initiate dfa
      dfa := 0.0;
      for k in 1 .. n loop
        dfa := dfa + d(k) * g(k);
      end loop;

      -- check for descent direction
      if dfa >= 0.0 then
        text_io.put("Line Search Failure");
        text_io.new_line;
        raise halt;
      end if;

      -- maxdf
      maxdf := -lstol * dfa;

      -- initiate fb
      for k in 1 .. n loop
        x(k) := x0(k) + d(k);
      end loop;
      objective(n,fb,x,g);

      -- initiate dfb
      dfb := 0.0;
      for k in 1 .. n loop
        dfb := dfb + d(k) * g(k);
      end loop;

      -- initiate interval
      a := 0.0;
      b := 1.0;
      while dfb < 0.0 and fb < fa loop
        a := b;
        fa := fb;
        dfa := dfb;

        b := b + b;

        -- fb
        for k in 1 .. n loop
          x(k) := x0(k) + b * d(k);
        end loop;
        objective(n,fb,x,g);

        -- dfb
        dfb := 0.0;
        for k in 1 .. n loop
          dfb := dfb + d(k) * g(k);
        end loop;
      end loop;

      -- repeat the interpolation
      ls := 1;
      loop
        -- cubic fit for a minimum
        alpha := cubic(a,b,fa,fb,dfa,dfb);

        -- f
        for k in 1 .. n loop
          x(k) := x0(k) + alpha * d(k);
        end loop;
        objective(n,f,x,g);

        -- df
        df := 0.0;
        for k in 1 .. n loop
          df := df + d(k) * g(k);
        end loop;

        exit when (f <= fa and f <= fb and abs df <= maxdf) or ls >= maxls;

        -- update ls
        ls := ls + 1;

        -- update interval
        if df < 0.0 then
          a := alpha;
          fa := f;
          dfa := df;
        else
          b := alpha;
          fb := f;
          dfb := df;
        end if;
      end loop;

      -- memory deallocation
      free_vec(x0);
    end linesearch;

    procedure lbfgs (maxlist,n,maxminor,maxls : in integer;
                     minor                    : out integer;
                     maxnorm,lstol            : in long_float;
                     f                        : out long_float;
                     x,g                      : in terms.fmat1_ptr;
                     optimal                  : out boolean) is

      -- limited memory self scaling BFGS

      type list_rec;

      type list_ptr is access list_rec;

      type list_rec (n : integer) is
        record
          alpha,sy  : long_float;
          s,y       : vec (1 .. n);
          prev,next : list_ptr;
        end record;

      pragma controlled(list_ptr);

      first,last : list_ptr := null;
      d,s,y      : vec_ptr;

      procedure free_rec is new unchecked_deallocation(list_rec,list_ptr);

      procedure create (first,last : out list_ptr;
                        n          : in integer;
                        s,y        : in vec_ptr) is

        -- create the doubly linked circular list

      begin
        last := new list_rec (n);

        last.sy := 0.0;
        for i in 1 .. n loop
          last.s(i) := s(i);
          last.y(i) := y(i);
          last.sy := last.sy + s(i) * y(i);
        end loop;

        last.next := last;
        last.prev := last;
        first := last;
      end create;

      procedure insert (first : in list_ptr;
                        last  : out list_ptr;
                        n     : in integer;
                        s,y   : in vec_ptr) is

        -- insert a new record on the doubly linked circular list

      begin
        last := new list_rec (n);

        last.sy := 0.0;
        for i in 1 .. n loop
          last.s(i) := s(i);
          last.y(i) := y(i);
          last.sy := last.sy + s(i) * y(i);
        end loop;

        last.next := first;
        last.prev := first.prev;
        first.prev.next := last;
        first.prev := last;
      end insert;

      procedure rotate (first,last : in out list_ptr;
                        n          : in integer;
                        s,y        : in vec_ptr) is

        -- discard the oldest record and insert a new record on
        -- the doubly linked circular list.

      begin
        first := first.next;
        last := last.next;

        last.sy := 0.0;
        for i in 1 .. n loop
          last.s(i) := s(i);
          last.y(i) := y(i);
          last.sy := last.sy + s(i) * y(i);
        end loop;
      end rotate;

      procedure destroy (first,last : in out list_ptr) is

        -- destroy the doubly linked circular list

        p : list_ptr;

      begin
        first := last;
        loop
          p := first;
          first := first.next;
          free_rec(p);
          exit when first = last;
        end loop;

        first := null;
        last := null;
      end destroy;

      procedure direction (first,last : in list_ptr;
                           g,d        : in vec_ptr) is

        p   : list_ptr;
        sum : long_float;

      begin
        -- two-loop scheme
        for i in 1 .. n loop
          d(i) := g(i);
        end loop;

        -- backward loop
        p := first;
        loop
          p := p.prev;

          sum := 0.0;
          for i in 1 .. n loop
            sum := sum + p.s(i) * d(i);
          end loop;
          p.alpha := sum / p.sy;

          for i in 1 .. n loop
            d(i) := d(i) - p.alpha * p.y(i);
          end loop;

          exit when p = first;
        end loop;

        -- scaling factor
        sum := 0.0;
        for i in 1 .. n loop
          sum := sum + last.y(i) ** 2;
        end loop;
        sum := last.sy / sum;

        for i in 1 .. n loop
          d(i) := sum * d(i);
        end loop;

        -- forward loop
        p := last;
        loop
          p := p.next;

          sum := 0.0;
          for i in 1 .. n loop
            sum := sum + p.y(i) * d(i);
          end loop;
          sum := p.alpha - sum / p.sy;

          for i in 1 .. n loop
            d(i) := d(i) + p.s(i) * sum;
          end loop;

          exit when p = last;
        end loop;

        -- search direction
        for i in 1 .. n loop
          d(i) := -d(i);
        end loop;
      end direction;

    begin
      -- memory allocation
      d := new vec (1 .. n);
      s := new vec (1 .. n);
      y := new vec (1 .. n);

      -- first iteration
      minor := 1;

      -- initiate f, g
      objective(n,f,x,g);

      -- search direction
      for i in 1 .. n loop
        d(i) := -g(i);
      end loop;

      -- initiate s, y
      for i in 1 .. n loop
        s(i) := x(i);
        y(i) := g(i);
      end loop;

      -- update f, x, g
      linesearch(n,maxls,lstol,f,x,g,d);

      -- s, y
      for i in 1 .. n loop
        s(i) := x(i) - s(i);
        y(i) := g(i) - y(i);
      end loop;

      -- create the doubly linked circular list
      create(first,last,n,s,y);

      while minor < maxminor and norm(n,g) > maxnorm loop
        minor := minor + 1;

        -- search direction
        direction(first,last,g,d);

        -- initiate s, y
        for i in 1 .. n loop
          s(i) := x(i);
          y(i) := g(i);
        end loop;

        -- update f, x, g
        linesearch(n,maxls,lstol,f,x,g,d);

        -- s, y
        for i in 1 .. n loop
          s(i) := x(i) - s(i);
          y(i) := g(i) - y(i);
        end loop;

        -- update the doubly linked circular list
        if minor > maxlist then
          rotate(first,last,n,s,y);
        else
          insert(first,last,n,s,y);
        end if;
      end loop;

      optimal := norm(n,g) <= maxnorm;

      -- memory deallocation
      free_vec(d);
      free_vec(s);
      free_vec(y);

      -- destroy the doubly linked circular list
      destroy(first,last);
    end lbfgs;

    procedure mbfgs (n,maxminor,maxls : in integer;
                     minor            : out integer;
                     maxnorm,lstol    : in long_float;
                     f                : out long_float;
                     x,g              : in terms.fmat1_ptr;
                     optimal          : out boolean) is

      -- memoryless self scaling BFGS

      pq,qq,pg,qg,c1,c2,gamma : long_float;
      d,p,q                   : vec_ptr;

    begin
      -- memory allocation
      d := new vec (1 .. n);
      p := new vec (1 .. n);
      q := new vec (1 .. n);

      -- first iteration
      minor := 1;

      -- initiate f, g
      objective(n,f,x,g);

      -- search direction
      for i in 1 .. n loop
        d(i) := -g(i);
      end loop;

      -- initiate p, q
      for i in 1 .. n loop
        p(i) := x(i);
        q(i) := g(i);
      end loop;

      -- update f, x, g
      linesearch(n,maxls,lstol,f,x,g,d);

      -- p, q
      for i in 1 .. n loop
        p(i) := x(i) - p(i);
        q(i) := g(i) - q(i);
      end loop;

      while minor < maxminor and norm(n,g) > maxnorm loop
        minor := minor + 1;

        -- search direction
        pq := 0.0;
        qq := 0.0;
        pg := 0.0;
        qg := 0.0;
        for i in 1 .. n loop
          pq := pq + p(i) * q(i);
          qq := qq + q(i) * q(i);
          pg := pg + p(i) * g(i);
          qg := qg + q(i) * g(i);
        end loop;

        gamma := pq / qq;
        c1 := qg / qq - 2.0 * pg / pq;
        c2 := pg / qq;

        for i in 1 .. n loop
          d(i) := c1 * p(i) + c2 * q(i) - gamma * g(i);
        end loop;

        -- initiate p, q
        for i in 1 .. n loop
          p(i) := x(i);
          q(i) := g(i);
        end loop;

        -- update f, x, g
        linesearch(n,maxls,lstol,f,x,g,d);

        -- p, q
        for i in 1 .. n loop
          p(i) := x(i) - p(i);
          q(i) := g(i) - q(i);
        end loop;
      end loop;

      optimal := norm(n,g) <= maxnorm;

      -- memory deallocation
      free_vec(d);
      free_vec(p);
      free_vec(q);
    end mbfgs;

    procedure bfgs (n,maxminor,maxls : in integer;
                    minor            : out integer;
                    maxnorm,lstol    : in long_float;
                    f                : out long_float;
                    x,g              : in terms.fmat1_ptr;
                    optimal          : out boolean) is

      -- self scaling BFGS

      pij                    : integer;
      sum,pq,qhq,c1,c2,gamma : long_float;
      d,p,q,hq,h             : vec_ptr;

      function pos (n,i,j : in integer) return integer is

      begin
        return (i - j) * (n + n + j - i + 1) / 2 + j;
      end pos;

      use
        ada.numerics.long_elementary_functions;

    begin
      -- memory allocation
      h := new vec (1 .. n * (n + 1) / 2);
      d := new vec (1 .. n);
      p := new vec (1 .. n);
      q := new vec (1 .. n);
      hq := new vec (1 .. n);

      -- first iteration
      minor := 1;

      -- initiate H
      for j in 1 .. n loop
        h(pos(n,j,j)) := 1.0;

        for i in j + 1 .. n loop
          h(pos(n,i,j)) := 0.0;
        end loop;
      end loop;

      -- initiate f, g
      objective(n,f,x,g);

      -- search direction
      for i in 1 .. n loop
        d(i) := -g(i);
      end loop;

      -- initiate p, q
      for i in 1 .. n loop
        p(i) := x(i);
        q(i) := g(i);
      end loop;

      -- update f, x, g
      linesearch(n,maxls,lstol,f,x,g,d);

      -- p, q
      for i in 1 .. n loop
        p(i) := x(i) - p(i);
        q(i) := g(i) - q(i);
      end loop;

      while minor < maxminor and norm(n,g) > maxnorm loop
        minor := minor + 1;

        -- calculate hq
        for i in 1 .. n loop
          sum := h(pos(n,i,i)) * q(i);

          for j in 1 .. i - 1 loop
            sum := sum + h(pos(n,i,j)) * q(j);
          end loop;

          for j in i + 1 .. n loop
            sum := sum + h(pos(n,j,i)) * q(j);
          end loop;

          hq(i) := sum;
        end loop;

        -- calculate pq, qhq
        pq := 0.0;
        qhq := 0.0;
        for i in 1 .. n loop
          pq := pq + p(i) * q(i);
          qhq := qhq + q(i) * hq(i);
        end loop;

        -- calculate BFGS vector
        c1 := sqrt(qhq);
        c2 := c1 / pq;
        for i in 1 .. n loop
          d(i) := c2 * p(i) - hq(i) / c1;
        end loop;

        -- gamma
        gamma := pq / qhq;

        -- update H
        for i in 1 .. n loop
          c1 := hq(i) / qhq;
          c2 := p(i) / pq;

          for j in 1 .. i loop
            pij := pos(n,i,j);
            h(pij) := (h(pij) - c1 * hq(j) + d(i) * d(j)) * gamma + c2 * p(j);
          end loop;
        end loop;

        -- search direction
        for i in 1 .. n loop
          sum := h(pos(n,i,i)) * g(i);

          for j in 1 .. i - 1 loop
            sum := sum + h(pos(n,i,j)) * g(j);
          end loop;

          for j in i + 1 .. n loop
            sum := sum + h(pos(n,j,i)) * g(j);
          end loop;

          d(i) := -sum;
        end loop;

        -- initiate p, q
        for i in 1 .. n loop
          p(i) := x(i);
          q(i) := g(i);
        end loop;

        -- update f, x, g
        linesearch(n,maxls,lstol,f,x,g,d);

        -- p, q
        for i in 1 .. n loop
          p(i) := x(i) - p(i);
          q(i) := g(i) - q(i);
        end loop;
      end loop;

      optimal := norm(n,g) <= maxnorm;

      -- memory deallocation
      free_vec(h);
      free_vec(d);
      free_vec(p);
      free_vec(q);
      free_vec(hq);
    end bfgs;
  end qnewton;
end nlp;
