+bool Elevator::currently_at_stop () const
+{
+ StopList::const_iterator it;
+ Stop current(position_, direction_);
+
+ /* Calculate the number of Stops above and below our current position */
+ int stops_above = 0;
+ int stops_below = 0;
+
+ for (it = stops_.begin(); it != stops_.end(); it++)
+ {
+ if (current < *it)
+ ++stops_above;
+
+ if (current > *it)
+ ++stops_below;
+ }
+
+ /* Check if we are at the top. If so, only the position needs to match */
+ if (direction_ == UP && stops_above == 0)
+ {
+ for (it = stops_.begin (); it != stops_.end (); it++)
+ if (it->getPosition() == position_)
+ return true;
+ }
+
+ /* Check if we are at the bottom. If so, only the position needs to match */
+ if (direction_ == DOWN && stops_below == 0)
+ {
+ for (it = stops_.begin (); it != stops_.end (); it++)
+ if (it->getPosition() == position_)
+ return true;
+ }
+
+ /* Check if we match exactly */
+ for (it = stops_.begin (); it != stops_.end (); it++)
+ if (*it == current)
+ return true;
+
+ /* Check if we are IDLE. If so, only the position needs to match */
+ if (direction_ == IDLE)
+ {
+ for (it = stops_.begin (); it != stops_.end (); it++)
+ if (it->getPosition() == position_)
+ return true;
+ }
+
+ /* No match */
+ return false;
+}
+
+void Elevator::stop_at (Stop &stop)
+{
+ StopList::iterator it;
+
+ /* If this is an "ALL" Stop, it supercedes all others */
+ if (stop.getDirection() == ALL)
+ {
+ stops_.remove (stop);
+ stops_.push_back (stop);
+ return;
+ }
+
+ /* Check if this stop already exists. If so, just leave */
+ for (it = stops_.begin(); it != stops_.end(); it++)
+ if (*it == stop)
+ return;
+
+ /* The stop did not already exist, so add it */
+ stops_.push_back (stop);
+}
+
+float Elevator::distance_from (Position &pos) const
+{
+ if (position_ > pos)
+ return position_ - pos;
+
+ return pos - position_;
+}
+
+float Elevator::distance_from (Stop &s) const
+{
+ Direction d = s.getDirection();
+ Position p = s.getPosition ();
+
+ /* If direction doesn't matter, then only position does */
+ if (d == ALL || direction_ == IDLE)
+ return distance_from (p);
+
+ /* If we're not in the same direction, then we're "really far" away */
+ if (d != direction_)
+ return INT_MAX;
+
+ /* We must be in the correct direction, so pure distance is fine */
+ return distance_from (p);
+}
+
+void Elevator::transition_move_up ()
+{
+ direction_ = UP;
+ position_ += ELEVATOR_STEP;
+
+ // TODO: Call into the GUI to update the position
+ gui_update_position_label (number_, (float)position_, direction_);
+ std::cout << "Updating the GUI with our position: " << position_ << std::endl;
+}
+
+void Elevator::transition_move_down ()
+{
+ direction_ = DOWN;
+ position_ -= ELEVATOR_STEP;
+
+ // TODO: Call into the GUI to update the position
+ gui_update_position_label (number_, (float)position_, direction_);
+ std::cout << "Updating the GUI with our position: " << position_ << std::endl;
+}
+
+void Elevator::transition_move_idle ()
+{
+ direction_ = IDLE;
+ // TODO: Call into the GUI to update the position
+ gui_update_position_label (number_, (float)position_, direction_);
+ // do not change position while IDLE
+}
+
+void Elevator::transition_open_door ()
+{
+ /* Calculate the number of Stops above and below our
+ * current position */
+ StopList::const_iterator it;
+ Stop current = Stop(position_, direction_);
+ int stops_above = 0;
+ int stops_below = 0;
+
+ for (it = stops_.begin(); it != stops_.end(); it++)
+ {
+ if (current < *it)
+ ++stops_above;
+
+ if (current > *it)
+ ++stops_below;
+ }
+
+ /* If we are going to switch direction, clear all stops here,
+ * regardless of direction.
+ *
+ * Otherwise, just clear this stop */
+ if (direction_ == UP && stops_above == 0)
+ {
+ stops_.remove (Stop(position_, ALL));
+ gui_unpress_request_button (number_, (int)position_);
+ gui_unpress_call_button ((int)position_, UP);
+ gui_unpress_call_button ((int)position_, DOWN);
+ }
+ else if (direction_ == DOWN && stops_below == 0)
+ {
+ stops_.remove (Stop(position_, ALL));
+ gui_unpress_request_button (number_, (int)position_);
+ gui_unpress_call_button ((int)position_, UP);
+ gui_unpress_call_button ((int)position_, DOWN);
+ }
+ else if (direction_ == IDLE)
+ {
+ stops_.remove (Stop(position_, ALL));
+ gui_unpress_request_button (number_, (int)position_);
+ gui_unpress_call_button ((int)position_, UP);
+ gui_unpress_call_button ((int)position_, DOWN);
+ }
+ else
+ {
+ stops_.remove (Stop(position_, direction_));
+ gui_unpress_call_button ((int)position_, direction_);
+ gui_unpress_request_button (number_, (int)position_);
+ }
+
+ // TODO: Call into the GUI to open the door
+ gui_open_door (number_, (int)position_);
+ std::cout << "Opening Door" << std::endl;
+}
+
+void Elevator::transition_close_door ()
+{
+ // TODO: Call into the GUI to close the door
+ gui_close_door (number_, (int)position_);
+ std::cout << "Closing Door" << std::endl;
+}
+
+void Elevator::transition_begin_wait ()
+{
+ wait_ = 10;
+}
+
+void Elevator::transition_continue_wait ()
+{
+ --wait_;
+}
+
+#include <string>
+static void debug (const std::string& s)
+{
+ std::cout << s << std::endl;
+}
+
+static std::string get_state_name (State s)
+{
+ std::string sname;
+
+ switch (s)
+ {
+ case STATE_IDLE:
+ sname = "STATE_IDLE";
+ break;
+ case STATE_UP:
+ sname = "STATE_UP";
+ break;
+ case STATE_DOWN:
+ sname = "STATE_DOWN";
+ break;
+ case STATE_WAIT:
+ sname = "STATE_WAIT";
+ break;
+ case STATE_OPEN_DOOR:
+ sname = "STATE_OPEN_DOOR";
+ break;
+ case STATE_CLOSE_DOOR:
+ sname = "STATE_CLOSE_DOOR";
+ break;
+ default:
+ sname = "BAD STATE";
+ break;
+ }
+
+ return sname;
+}
+
+static std::string get_event_name (Event e)