Index: trunk/code/projects/warehouse/WH_Robot.h
===================================================================
--- trunk/code/projects/warehouse/WH_Robot.h	(revision 2002)
+++ trunk/code/projects/warehouse/WH_Robot.h	(revision 2003)
@@ -5,29 +5,24 @@
 #define TASK_COMPLETED 0
 #define TASK_FAILED -1
 
-#include "../Behavior.h"
 #include "navigationMap.h"
 #include "Scheduler.h" 
-#include "../helper_classes/Order.h"
+#include "/helper_classes/Order.h"
 #include <assert.h>
 #include <stdlib.h>
 
-class WH_Robot : Behavior{
 
-        Order* curr_task;
-        navigationMap nav_map;
-        Scheduler scheduler;
+Order* curr_task;
+navigationMap nav_map;
+Scheduler scheduler;
 
-        Time get_wc_time(State dest);
-        int exec_task();
+Time get_wc_time(State dest);
+int exec_task();
 
-    public:
-        WH_Robot(std::string scoutname);
-        ~WH_Robot();
-        void run();
+WH_Robot(std::string scoutname);
+~WH_Robot();
+void run();
 
-        void set_task(Order order);
+void set_task(Order order);
 
-};
-
 #endif
Index: trunk/code/projects/warehouse/helper_classes/Order.h
===================================================================
--- trunk/code/projects/warehouse/helper_classes/Order.h	(nonexistent)
+++ trunk/code/projects/warehouse/helper_classes/Order.h	(revision 2003)
@@ -0,0 +1,69 @@
+#include "Order.h"
+
+using namespace std;
+
+/** @Brief: Regular order constructor */
+Order::Order(int order_id, Address order_source, Address order_dest, 
+    Time order_start_time, Path order_path, Time order_est_time) 
+{ 
+    id = order_id;
+    source = order_source;
+    dest = order_dest;
+    start_time = order_start_time;
+    path = order_path;
+    est_time = order_est_time;
+}
+
+/** @Brief: Get order ID */
+int Order::getid() const 
+{
+    return id;
+}  
+
+Address Order::get_source() const
+{
+    return source;
+}
+
+Address Order::get_dest() const
+{
+    return dest;
+}
+
+Time Order::get_start_time() const
+{
+    return start_time;
+}
+
+Path Order::get_path() const
+{
+    return path;
+}
+
+Time Order::get_est_time() const
+{
+    return est_time;
+}
+
+bool Order::operator==(Order& o1, Order& o2)
+{
+  return o1.id == o2.id;
+}
+
+void Order::set_path(Path order_path)
+{
+    path = order_path;
+    return;
+}
+/** @Brief: Order comparison function for PQWrapper 
+ *  NOTE: In order to have a min priority queue, using c++'s pq
+ *  implementation, the compare function must return true if
+ *  o1 is greater than o2.
+ */
+bool CompareOrder::operator()(Order& o1, Order& o2) 
+{
+  int pq_value1 = o1.get_start_time + MAX_WAIT_TIME - o1.get_est_time();
+  int pq_value2 = o2.get_start_time + MAX_WAIT_TIME - o2.get_est_time();
+  return pq_value1 > pq_value2;
+}
+
Index: trunk/code/projects/warehouse/helper_classes/PQWrapper.cpp
===================================================================
--- trunk/code/projects/warehouse/helper_classes/PQWrapper.cpp	(nonexistent)
+++ trunk/code/projects/warehouse/helper_classes/PQWrapper.cpp	(revision 2003)
@@ -0,0 +1,176 @@
+#include "PQWrapper.h"
+
+using namespace std;
+
+/** @Brief: Initialize PQWrapper data structures. */
+PQWrapper::PQWrapper(unsigned int numElems)
+{
+    minElems = new Order[numElems];
+    arrCapacity = numElems;
+    arrContents = 0;
+}
+
+/** @Brief: Free allocatetd memory. */
+PQWrapper::~PQWrapper()
+{
+    while (!pq.empty()) pq.pop();
+}
+
+/** @Brief: Insert an order into the PQWrapper. */
+void PQWrapper::insert(Order o)
+{
+
+    int position=0;
+    bool foundPosition = false;
+    while (!foundPosition && position < arrContents)
+    {
+        if (o.getpriority() < minElems[position].getpriority()) 
+        {
+            foundPosition=true;
+        } 
+        else 
+        {
+            position++;
+        }
+    }
+    if (foundPosition)
+    {
+        if (arrContents<arrCapacity)
+        {
+            for (int i = arrContents; i >=position+1; i--) 
+            {
+                minElems[i] = minElems[i-1];
+            }            
+            minElems[position] = o;
+            arrContents++;
+        } 
+        else 
+        {
+            pq.push(minElems[arrCapacity-1]);
+            for (int i = arrContents-1; i >= position+1; i--) 
+            {
+                minElems[i] = minElems[i-1];
+            }            
+            minElems[position] = o;
+        }
+    } 
+    else 
+    {     
+        if (arrContents<arrCapacity) 
+        {
+            minElems[arrContents] = o;
+            arrContents++;
+        } else {
+            pq.push(o);
+        }
+    }
+}
+
+/** @Brief: Remove and return an Order from the array if 0 <= index < arrContents. */
+Order PQWrapper::remove(unsigned int index)
+{
+    if (index >= arrContents) return minElems[-1];//TODO: can I return null?
+    Order toDelete = minElems[index];
+    for (int i = index; i < arrContents - 1; i++) 
+    {
+        minElems[i] = minElems[i+1];
+    }
+    if (!pq.empty()) 
+    {
+        minElems[arrContents-1] = pq.top();
+        pq.pop();
+    } 
+    else 
+    {
+        arrContents--;
+    }
+    return toDelete;
+}
+
+/** @Brief: Return an Order from the array if 0 <= index < arrContents */
+Order PQWrapper::peek(unsigned int index) const
+{
+    if (index >= arrContents)
+    {
+        return minElems[-1];//TODO: can I return null?
+    } 
+    else 
+    {
+        return minElems[index];
+    }
+}
+
+/** @Brief: Return the size of the minElems array */
+unsigned int PQWrapper::arraySize() const
+{
+    return arrContents;
+}
+
+
+/*
+//For testing purposes only
+int main() {
+    Order a(1,0,0,101,0,0,0);
+    Order b(2,0,0,11,0,0,0);
+    Order c(3,0,0,91,0,0,0);
+    Order d(4,0,0,21,0,0,0);
+    Order e(5,0,0,41,0,0,0);
+    Order f(6,0,0,81,0,0,0);
+    Order g(7,0,0,31,0,0,0);
+    Order h(8,0,0,71,0,0,0);
+    Order i(9,0,0,51,0,0,0);
+    Order j(10,0,0,61,0,0,0);
+
+    PQWrapper pqw(5);
+
+    pqw.insert(a);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+
+    pqw.insert(b);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(c);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(d);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(e);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(f);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(g);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(h);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(i);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    pqw.insert(j);
+    for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+    cout << endl; 
+
+    for (int i=0;i<10;i++) 
+    {
+        pqw.remove(0);
+        for (int i=0;i<pqw.arraySize();i++) cout << pqw.peek(i).getid() << " ";
+        cout << endl; 
+    }
+
+    return 0;
+}
+*/
Index: trunk/code/projects/warehouse/helper_classes/PQWrapper.h
===================================================================
--- trunk/code/projects/warehouse/helper_classes/PQWrapper.h	(nonexistent)
+++ trunk/code/projects/warehouse/helper_classes/PQWrapper.h	(revision 2003)
@@ -0,0 +1,25 @@
+#ifndef _PQ_WRAPPER_
+#define _PQ_WRAPPER_
+
+#include <cstdlib>
+#include <queue>
+#include <iostream>
+#include "Order.h"
+
+class PQWrapper
+{
+    public:
+        PQWrapper(unsigned int numElems);
+        ~PQWrapper();
+        
+        void insert(Order o);
+        Order remove(unsigned int index);
+        Order peek(unsigned int index) const;
+        unsigned int arraySize() const;
+
+    private:
+        Order* minElems;
+        unsigned int arrCapacity, arrContents;
+        std::priority_queue<Order,std::vector<Order>, CompareOrder> pq;
+};
+#endif
Index: trunk/code/projects/warehouse/helper_classes/Order.cpp
===================================================================
--- trunk/code/projects/warehouse/helper_classes/Order.cpp	(nonexistent)
+++ trunk/code/projects/warehouse/helper_classes/Order.cpp	(revision 2003)
@@ -0,0 +1,69 @@
+#include "Order.h"
+
+using namespace std;
+
+/** @Brief: Regular order constructor */
+Order::Order(int order_id, Address order_source, Address order_dest, 
+    Time order_start_time, Path order_path, Time order_est_time) 
+{ 
+    id = order_id;
+    source = order_source;
+    dest = order_dest;
+    start_time = order_start_time;
+    path = order_path;
+    est_time = order_est_time;
+}
+
+/** @Brief: Get order ID */
+int Order::getid() const 
+{
+    return id;
+}  
+
+Address Order::get_source() const
+{
+    return source;
+}
+
+Address Order::get_dest() const
+{
+    return dest;
+}
+
+Time Order::get_start_time() const
+{
+    return start_time;
+}
+
+Path Order::get_path() const
+{
+    return path;
+}
+
+Time Order::get_est_time() const
+{
+    return est_time;
+}
+
+bool Order::operator==(Order& o1, Order& o2)
+{
+  return o1.id == o2.id;
+}
+
+void Order::set_path(Path order_path)
+{
+    path = order_path;
+    return;
+}
+/** @Brief: Order comparison function for PQWrapper 
+ *  NOTE: In order to have a min priority queue, using c++'s pq
+ *  implementation, the compare function must return true if
+ *  o1 is greater than o2.
+ */
+bool CompareOrder::operator()(Order& o1, Order& o2) 
+{
+  int pq_value1 = o1.get_start_time + MAX_WAIT_TIME - o1.get_est_time();
+  int pq_value2 = o2.get_start_time + MAX_WAIT_TIME - o2.get_est_time();
+  return pq_value1 > pq_value2;
+}
+
