/*
Copyright 2011-2013 Frederic Langlet
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
you may obtain a copy of the License at

                http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

package kanzi.filter.seam;

// Not thread safe
// Tree based sorted queue of geodesics
/*package*/ class GeodesicSortedQueue
{
    private final int maxSize;
    private int size;
    private Node head;
    private Node tail;
    private Node root;
    private final Node[] nodes;
    private int freeNodeIdx;


    public GeodesicSortedQueue(int maxSize)
    {
        this.maxSize = maxSize;
        this.nodes = new Node[maxSize+1];

        // Pre-allocate
        for (int i=0; i<this.nodes.length; i++)
           this.nodes[i] = new Node(null, null, i);
    }


    // Grow queue until maxSize is reached
    // Return last value (geodesic with highest cost) in ordered collection
    // Null input value is not allowed
    public Geodesic add(Geodesic value)
    {
        if (this.size == 0)
        {
           Node node = this.nodes[this.freeNodeIdx++];
           node.value = value;
           this.head = node;
           this.tail = node;
           this.root = node;
           this.size++;
           return this.tail.value;
        }

        final int cost = value.cost; // aliasing

        if (cost >= this.tail.value.cost)
        {
           // Cost too high, do not add
           if (this.size == this.maxSize)
             return this.tail.value;

           // New node is tail
           Node node = this.nodes[this.freeNodeIdx++];
           node.value = value;
           node.parent = this.tail;
           this.tail.right = node;
           this.tail = node;
        }
        else if (cost < this.head.value.cost)
        {
           // New node is head
           Node node = this.nodes[this.freeNodeIdx++];
           node.value = value;
           node.parent = this.head;
           this.head.left = node;
           this.head = node;
        }
        else
        {
           // New node is not an extremity
           Node current = this.root;

           // Locate appropriate position in tree
           while (true)
           {
              if (cost > current.value.cost)
              {
                 if (current.right != null)
                   current = current.right;
                 else
                 {
                    Node node = this.nodes[this.freeNodeIdx++];
                    node.value = value;
                    node.parent = current;
                    current.right = node;
                    break;
                 }
              }
              else
              {
                 if (current.left != null)
                   current = current.left;
                 else
                 {
                    Node node = this.nodes[this.freeNodeIdx++];
                    node.value = value;
                    node.parent = current;
                    current.left = node;
                    break;
                 }
              }
           } 
        }

        if (this.size >= this.maxSize)
        {
           // Need to recompute tail
           Node last = this.tail;

           // Recycle evicted node
           this.freeNodeIdx = last.idx;

           if (last.left != null)
           {
               final Node left = last.left;
               final Node parent = last.parent;
               left.parent = parent;

               if (parent != null)
                  parent.right = left;
               else
                  this.root = left;
               
               last.parent = null;
               last.left = null;
               last = left;

               while (last.right != null)
                   last = last.right;

               // Set new tail to rightmost descendant of 'left'
               this.tail = last;
           }
           else
           {
              // Set new tail to parent of current tail
              this.tail = last.parent;
              this.tail.right = null;
              last.parent = null;
           }
        }
        else
        {
           this.size++;
        }

        return this.tail.value;
    }


    public boolean isFull()
    {
        return (this.size == this.maxSize);
    }


    public Geodesic getLast()
    {
        if (this.size == 0)
            return null;

        return this.tail.value;
    }


    public Geodesic getFirst()
    {
        if (this.size == 0)
            return null;

        return this.head.value;
    }


    public int size()
    {
        return this.size;
    }


    public Geodesic[] toArray(Geodesic[] array)
    {
        if (this.size == 0)
           return new Geodesic[0];

        if (array.length < this.size)
           array = new Geodesic[this.size];

        scan(this.root, array, 0);
        return array;
    }


    private static int scan(Node n, Geodesic[] array, int idx)
    {
       if (n.left != null)
          idx = scan(n.left, array, idx);

       array[idx++] = n.value;

       if (n.right != null)
          idx = scan(n.right, array, idx);

       return idx;
    }



    private static class Node
    {
        Node left;
        Node right;
        Node parent;
        Geodesic value;
        final int idx;

        Node(Node parent, Geodesic value, int idx)
        {
            this.parent = parent;
            this.value = value;
            this.idx = idx;
        }
    }


    @Override
    public String toString()
    {
       StringBuilder builder = new StringBuilder((1+this.size)*8);
       Geodesic[] array = this.toArray(new Geodesic[this.size]);

       builder.append("Size=");
       builder.append(this.size);
       builder.append("\n");

       for (int i=0; i<array.length; i++)
       {
          if (i != 0)
             builder.append(",\n");

          builder.append(array[i]);
       }

       return builder.toString();
    }

}


@@ -828,32 +828,8 @@
 l;%0D%0A
-    private Node root;%0D%0A
     
@@ -857,16 +857,16 @@
 nodes;%0D%0A
+
     priv
@@ -1560,38 +1560,8 @@
 e;%0D%0A
-           this.root = node;%0D%0A
     
@@ -2045,32 +2045,63 @@
 .right = node;%0D%0A
+           node.right = null;%0D%0A
            this.
@@ -2306,36 +2306,41 @@
 ue;%0D%0A           
-node
+this.head
 .parent = this.h
@@ -2328,24 +2328,55 @@
 ead.parent =
+ node;%0D%0A           node.right =
  this.head;%0D
@@ -2387,25 +2387,20 @@
         
-this.head
+node
 .left = 
@@ -2392,35 +2392,35 @@
    node.left = n
-ode
+ull
 ;%0D%0A           th
@@ -2547,20 +2547,20 @@
  = this.
-root
+head
 ;%0D%0A%0D%0A   
@@ -4021,66 +4021,8 @@
 t;%0D%0A
-               else%0D%0A                  this.root = left;%0D%0A
     
@@ -5337,12 +5337,12 @@
 his.
-root
+head
 , ar

Fix incorrect update of the list head