Colony

#include "Scheduler.h"

using namespace std;

/** @Brief: Initialize data structures for the scheduler. */

Scheduler::Scheduler()

{

        WH_Robot rob1(/*params*/);

        WH_Robot rob2(/*params*/);

        WH_Robot rob3(/*params*/);

        robots.insert(rob1);

        robots.insert(rob2);

        robots.insert(rob3);

        create_orders();

}

/** @Brief: Free allocatetd memory. */

Scheduler::~Scheduler()

{

   delete rob1;

   delete rob2;

   delete rob3;

   delete robots;

   delete unassignedOrders;

   delete assigned Orders;

   delete waitingRobots;

}

/** @Brief: Add robot to the waiting queue. 

 *  A robot calls this function with itself

 *  and gets pushed on a list of waiting robots.

 *  When a task is availaible the scheduler, removes

 *  the robot of the waiting queue, and gives it a

 *  task.

 */

void Scheduler::get_task(WH_Robot r)

{

        waitingRobots.push(r);

}

/** @Brief: Statically add orders to the Priority Queue Wrapper.*/

void Scheduler::create_orders()

{

        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);

        unassignedOrders.insert(a);

        unassignedOrders.insert(b);

        unassignedOrders.insert(c);

        unassignedOrders.insert(d);

        unassignedOrders.insert(e);

}

/** @Brief: This is a confirmation that the task is complete. 

    This function removes the order from assignedOrders. */

void Scheduler::task_complete(Order o)

{

    for (int i=0; i< assignedOrders.size(); i++)

        {

            if (assignedOrders[i].getid()==o.getid())

                {

                        assignedOrders.erase(assignedOrders.begin()+i);

                }

        }

}

/** @Brief: This is a confirmation that the task failed. 

    This function places the order back on the PQWrapper. */

void Scheduler::task_failed(Order o)

{

        task_complete(o);

        unassignedOrders.insert(o);

}

/** @Brief: Do a waiting dance. */

void Scheduler::waiting_dance()

{

}

/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */

Order Scheduler::get_next_item()

{

  Time time = ros::TIME_MAX; 

  Order best;

  for(int i=0; i<unassigned_orders.arraySize(); i++)

  {

    Order order = unassigned_orders.peek(i);

    if(order.orderDeadline < time) //TODO: use another function

    {

      best = order;

      time = order.orderDeadline;

    }

  }

}

/** @Brief: Continuously checks for waiting robots. If no robots are waiting,

    this function calls the waiting_dance() function. */

void Scheduler::run()

{

        while (ok())

        {

                while(waitingRobots.empty() || unassignedOrders.arraySize()==0) 

      waiting_dance();

                waitingRobots.front().set_task(get_next_item());

                waitingRobots.pop();

        }

}