#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include "lift.h"

/* some global stats */
int total_waiting = 0;
int no_of_waits = 0;
int max_wait = 0;
int min_wait = 10000000; 

/*
 * Initialize the floors
 */
void init_floors(struct floor_s* floors) 
{
  int i = 0;
  for(i = 0; i < NO_OF_FLOORS; i++) {
    floors[i].floor_number = i;
    floors[i].num_waiting = 0;
    floors[i].num_not_waiting = 0;
  }
}

/*
 * arrive_on_floor is called when a person arrives at a new floor.
 */
void arrive_on_floor(int time, struct floor_s* floor, struct person_s* person)
{
    person->target_floor = -1;  /* just arrived, don't want to go
				   anywhere yet */
    person->floor = floor->floor_number;

    /* store them in the first free slot in the array */
    floor->people_not_waiting[floor->num_not_waiting] = person;
    floor->num_not_waiting++;

    if (person->start_waiting != -1) {
      total_waiting += time - person->start_waiting;
      printf("Their journey time was %d\n", time - person->start_waiting);
      if (time - person->start_waiting > max_wait) {
	max_wait = time - person->start_waiting;
      }
      if (time - person->start_waiting < min_wait) {
	min_wait = time - person->start_waiting;
      }
      no_of_waits++;
      person->start_waiting = -1;
    }
}

/* 
 * init_people initialized the people to start from the ground floor 
 */
void init_people(struct floor_s* floors, struct person_s* people) 
{
  int i = 0;
  for(i = 0; i < NO_OF_PEOPLE; i++) {
    people[i].name[0] = 'A' + i;
    people[i].name[1] = '\0';
    people[i].start_waiting = -1;
    arrive_on_floor(0, &floors[0], &people[i]);
  }
}

/* 
 * init_lists initializes the lifts to start from the ground floor
 */
void init_lifts(struct lift_s* lifts)
{
  int i;
  for (i = 0; i < NO_OF_LIFTS; i++) {
    lifts[i].lift_id = i;
    lifts[i].occupants = 0;
    lifts[i].position = 0;
    lifts[i].speed = 0;
    lifts[i].last_floor = 0;
    lifts[i].mode = MODE_IDLE;
  }
}

/*
 * queue_for_list is called to add a person who was not previously
 * queuing for a lift to the lift queue on their current floor
 */
void queue_for_lift(struct person_s* person, struct floor_s* floor)
{
  int i, j = -1;

  /* find the person and remoove them from the pool of people not
     queuing */
  for (i = 0; i < floor->num_not_waiting; i++) {
    if (floor->people_not_waiting[i] == person) {
      for (j = i; j < floor->num_not_waiting-1; j++) {
      
	floor->people_not_waiting[j] = floor->people_not_waiting[j+1];
      }
      break;
    }
  }
  assert(j != -1);
  floor->num_not_waiting--;
  floor->people_not_waiting[floor->num_not_waiting] = NULL;

  /* add them to the queue of people waiting for the lift */
  floor->people_waiting[floor->num_waiting] = person;
  floor->num_waiting++;
}

/*
 * decide_action is called to allow a person to randomly decide if
 * they want to go to another floor 
 */
void decide_action(int time, struct person_s* person, struct floor_s* floor)
{
  if (person->target_floor != -1) 
    return;  /* they've already decided where they're going */

  /* give then a 5% change of deciding to use the lift */
  if (random()%100 > 95) {

    /* choose a random floor (but not the one they're already on) */
    person->target_floor = floor->floor_number;
    while (person->target_floor == floor->floor_number) {
      person->target_floor = random()%NO_OF_FLOORS;
    }

    /* add them to the queue */
    queue_for_lift(person, floor);
    printf("Person %s on floor %d decides to go to floor %d\n", person->name,
	   person->floor, person->target_floor);

    /* the wait starts here */
    person->start_waiting = time;
    return;
  } 
  
  /* otherwise do nothing */
}

/*
 * person_enters_lift is called to move a person from a floor to a lift
 */
void person_enters_lift(int time, struct lift_s* lift, struct floor_s* floor) {
  struct person_s * person;
  int i;
  assert(floor->num_waiting > 0);

  person = floor->people_waiting[0];
  /* shuffle everyone else up the queue */
  for (i = 0; i < floor->num_waiting-1; i++) {
    floor->people_waiting[i] = floor->people_waiting[i+1];
  }
  floor->num_waiting--;
  floor->people_waiting[floor->num_waiting] = NULL;

  printf("Person %s enters lift %d on floor %d\n", 
	 person->name, lift->lift_id, floor->floor_number);
  lift->people[lift->occupants] = person;
  lift->occupants++;
}

/* 
 * people_enter_lift is called to move as many people as will fit from
 * a floor queue to a lift
 */
void people_enter_lift(int time, struct lift_s* lift, struct floor_s* floor) {
  while (floor->num_waiting > 0
	 && lift->occupants < MAX_PEOPLE_IN_LIFT) {
    person_enters_lift(time, lift, floor);
  }
}

/*
 * person_leaves_lift is called to move a person from a lift to their
 * target floor (when the lift is at that floor)
 */
void person_leaves_lift(int time, struct person_s* person, 
			struct floor_s* floor, struct lift_s* lift) {
  printf("Person %s leaves lift %d on floor %d\n", 
	 person->name, lift->lift_id, floor->floor_number);
  arrive_on_floor(time, floor, person);
}

/*
 * people_leave_lift is called to move people from a lift to their
 * target floor (when the lift is at that floor)
 */
void people_leave_lift(int time, struct lift_s* lift, struct floor_s* floor) {
  int i, j;
  for (i = 0; i < lift->occupants; i++) {
    while (lift->people[i]->target_floor == floor->floor_number) {
      person_leaves_lift(time, lift->people[i], floor, lift);

      /* shuffle everyone else up to fill the gap */
      for (j = i; j < lift->occupants-1; j++) {
	lift->people[j] = lift->people[j+1];
      }
      lift->occupants--;
    }
  }
}

/*
 * distance_to_target calculates how far the lift still has to go to
 * reach its target floor
 */
int distance_to_target(struct lift_s* lift)
{
  int target_position, dist;
  target_position = lift->next_floor * FEET_PER_FLOOR;
  dist = target_position - lift->position;
  return abs(dist);
}

/*
 * braking_distance calculates the distance a lift will take to stop
 * if it tries to stop from the given speed
 */
int braking_distance(int speed)
{
  int dist = 0;
  speed = abs(speed);
  while(speed > 0) {
    dist += speed;
    speed--;
  }
  return dist;
}


/* 
 * speed_up is called to accelerate a lift
 */
void speed_up(struct lift_s* lift)
{
  if (lift->speed == 0) {
    /* we weren't moving, so figure start in the right direction */
    if (lift->next_floor * FEET_PER_FLOOR > lift->position) 
      lift->speed = 1;
    else 
      lift->speed = -1;
  } else if (lift->speed > 0) {
    /* going up */
    if (lift->speed < MAX_SPEED)
      lift->speed++;
  } else {
    /* going down, so speed must become more negative */
    if (abs(lift->speed) < MAX_SPEED)
      lift->speed--;
  }
}

/* 
 * slow_down is called to deccelerate a lift
 *
 */
void slow_down(struct lift_s* lift)
{
  if (lift->speed == 0)
    /* can't slow down, we're already stopped */
    return;
  if (lift->speed > 0)
    /* going up */
    lift->speed--;
  else
    /* going down */
    lift->speed++;
}

/* 
 * current_floor returns the number of the floor that a lift is stopped at
 */
int current_floor(struct lift_s* lift)
{
  assert(lift->position % FEET_PER_FLOOR == 0);
  return lift->position/FEET_PER_FLOOR;
}

/*
 * button_pressed returns TRUE if any lift button has been pressed on
 * any floor.
 */
int button_pressed(struct floor_s* floors) 
{
  int i;
  for (i = 0; i < NO_OF_FLOORS; i++) {
    if (floors[i].num_waiting > 0) {
      printf("Button pressed: TRUE\n");
      return TRUE; 
    }
  }
  /* printf("Button pressed: FALSE\n"); */
  return FALSE; 
}

/*
 * decide_next_floor is called when the lift doors have closed, and a
 * lift is about to depart.  It chooses the next floor the lift will
 * stop at.  Essentially it is the lift scheduling algorithm, and so
 * this determines the performance of the lift system
 */
int decide_next_floor(struct lift_s* lift, struct floor_s* floors)
{
  int closest_floor = -1, this_floor, i;
  
  /*
  int current_floor(struct lift_s* lift)
{
  assert(lift->position % FEET_PER_FLOOR == 0);
  return lift->position/FEET_PER_FLOOR;
}
*/
  this_floor = current_floor(lift);
  printf("Lift %d (on floor %d) is deciding the next floor\n",
	 lift->lift_id, this_floor);

  /* is there anyone in the lift?  if so go where they want */
  /* find the closest floor from those that the people in the lift
     want to visit */
  if (lift->occupants > 0) {
    for (i = 0; i < lift->occupants; i++) {
      printf("Passenger %s chose floor %d\n", lift->people[i]->name,
	    lift->people[i]->target_floor);
      if (closest_floor == -1) {
	       /* first time through the loop */
	       closest_floor = lift->people[i]->target_floor;
      } else if ( abs(lift->people[i]->target_floor - this_floor) <
		     abs(closest_floor - this_floor) ) {
	       /* this floor is closer than the previous closest */
	       closest_floor = lift->people[i]->target_floor;
      }
    }
    printf("The closest chosen floor is %d\n", closest_floor);
    assert(closest_floor != -1);
    return closest_floor;
  }

  /* there's no-one in the lift, so we must choose from one of the
     floors where people are waiting */
  for (i = 0; i < NO_OF_FLOORS; i++) {
    printf("There's no-one in the lift\n");
    if (i == this_floor) continue; /* must move to a different floor */
    if (floors[i].num_waiting > 0) {
      printf("Button is pressed on floor %d\n", i);
    }
    if ( (floors[i].num_waiting > 0) &&
	 (abs(i - this_floor) < abs(closest_floor - this_floor) 
	 || closest_floor == -1)) {
      closest_floor = i;
    }
  }

  /* if we haven't got an answer, don't move the lift */
  if (closest_floor == -1)
    closest_floor = this_floor;
  else
    printf("The closest chosen floor is %d\n", closest_floor);
  return closest_floor;
}


/*
 * move_lift implements the full lift state machine.  The doors open,
 * then they stay open for a little while, during which people leave
 * and enter the lift.  The doors close, then the lift decides when
 * floor to visit next.  It accelerates, then it decelerates, stops,
 * and the doors open again.
 */
void move_lift(int time, struct lift_s* lift, struct floor_s* floors) 
{
  int distance_to_go;

  switch(lift->mode) {
  case MODE_OPENING:
    if (lift->timer < DOOR_OPENING_DELAY) {
      /* doors are opening, but not yet fully open */
      lift->timer++;
    } else {
      /* doors are fully open */
      lift->timer = 0;
      lift->mode = MODE_OPEN;
      printf("Lift %d, doors now open\n", lift->lift_id);
    }
    break;
  case MODE_OPEN:
    /* allow people in while there is space */
    people_leave_lift(time, lift, &floors[current_floor(lift)]);
    people_enter_lift(time, lift, &floors[current_floor(lift)]);

    /* close the doors if the timer has expired */
    if (lift->timer < DOOR_OPEN_DELAY) {
      /* not yet time */
      lift->timer++;
    } else {
      /* time to close the doors */
      lift->timer = 0;
      printf("Lift %d, doors starting to close\n", lift->lift_id);
      lift->mode = MODE_CLOSING;
    }
    break;
  case MODE_CLOSING:
    /* model doors closing */
    if (lift->timer < DOOR_CLOSING_DELAY) {
      /* doors closing, but not yet closed */
      lift->timer++;
      break;
    }

    /* doors are closed, what happens next? */
    printf("Lift %d, doors now closed\n", lift->lift_id);
    lift->timer = 0;
    if (lift->occupants == 0 && button_pressed(floors) == FALSE) {
      /* there's nowhere to go, so just go idle */
      lift->mode = MODE_IDLE;
      lift->next_floor = -1;
      printf("Lift %d Idle\n", lift->lift_id);
    } else {
      /* start the lift moving */
      lift->next_floor = decide_next_floor(lift, floors);
      lift->last_floor = current_floor(lift);
      lift->mode = MODE_MOVING;
      printf("Lift %d starting towards floor %d\n", 
	     lift->lift_id, lift->next_floor);
    }
    break;
  case MODE_MOVING:
    /* lift is moving; control the speed to get there quickly without
       overshooting */
    lift->position += lift->speed;
    printf("Lift %d moving, position: %d, speed %d\n", 
	   lift->lift_id, lift->position, lift->speed);
    distance_to_go = distance_to_target(lift);
    if (distance_to_go == 0) {
      lift->speed = 0;
      lift->next_floor = -1;
      lift->timer = 0;
      lift->mode = MODE_OPENING;
      printf("Lift %d arrived at position %d, doors opening\n",
	     lift->lift_id, lift->position);
    } else if (distance_to_go > 
	       (abs(lift->speed) + braking_distance(lift->speed))) {
      /* speed up */
      speed_up(lift);
    } else if (distance_to_go >= braking_distance(lift->speed)){
      /* this speed is OK */
    } else {
      /* slow_down */
      slow_down(lift);
    }
    break;
  case MODE_IDLE:
    /* the lift is idle */
    if (button_pressed(floors) == TRUE) {
      /* someone called the lift */
      lift->next_floor = decide_next_floor(lift, floors);
      lift->mode = MODE_MOVING;
      printf("Lift %d starting towards floor %d\n", 
	     lift->lift_id, lift->next_floor);
    }
    break;
  }
}

/*
 * main loop, all the main paramaters are in lift.h
 */

int main(int arc, char **argv) 
{
  int time;
  struct floor_s floors[NO_OF_FLOORS];
  struct person_s people[NO_OF_PEOPLE];
  struct lift_s lifts[NO_OF_LIFTS];

  init_floors(floors);
  init_people(floors, people);
  init_lifts(lifts);

  srandom(0);  /* seed the random number generator so we always get
		  the same answer */

  /* run the simulator for the prescribed time */
  for (time = 0; time < MAX_TIME; time++) {
    int i;
    printf("t=%d\n", time);
    for (i = 0; i < NO_OF_PEOPLE; i++) {
      if (people[i].floor != -1) {
	/* give each person a chance to decide what to do */
	decide_action(time, &people[i], &floors[people[i].floor]);
      } else {
	/* they're in the lift, so they don't get a choice */
      }
    }
    for (i = 0; i < NO_OF_LIFTS; i++) {
      move_lift(time, &lifts[i], floors);
    }
  }

  printf("-------------------------------------------------------------\n");
  printf("Lift Statistics:\n");
  printf("  Total waiting: %d\n", total_waiting);
  printf("  No. of completed journeys: %d\n", no_of_waits);
  printf("  Mean wait time: %f\n", (double)total_waiting / no_of_waits);
  printf("  Min wait time: %d\n", min_wait);
  printf("  Max wait time: %d\n", max_wait);
	 
  exit(0);
}