//Andrew Costello (aoc6)
//Dylan Hughes (dah64)
//ECE 476 Final Project
//4/30/2007

#include <Mega32.h>
#include <stdio.h> // sprintf
#include <stdlib.h> // malloc
#include <delay.h>
#include <Math.h>
#define begin {
#define end }
#define floorsize 38
#define pause 75
#define trivial 0 //one elevator does everything
#define simple 1 //all elevators chase each other
#define normal 2 //optimal elevator operation
#define sabbath 3 //one or all elevators dedicated to going up/down, stopping on all floors

#define floors 4
int mode = normal;

typedef struct
{
    int floor;//current floor
    int traveldir;//current direction of travel -1 = down, 1 = up;
    char turnaround;//floor where the elevator should switch directions
    int count;//used for interrupt to determine how far to send elevator
    char motorstate;//state of step motor
    char button[floors];//buttons in elevator that have been pressed
    char uprequests[floors];//requests from people outside the elevator to go up
    char oldbutton[floors];
    char downrequests[floors];//requests from people outside the elevator to go down
    int paused;//number of interrupts to pause elevator for
    char on;//whether elevator should move 1 stepper cycle
    char id;//elevator #
} Elevator;

typedef Elevator *elevator_t;//elevator_t is a pointer to type Elevator
void algorithm1(elevator_t);
void addRequests(void);
void oneStep(elevator_t);
void algorithm2(void);
void algorithm3(elevator_t);
void poll2(void);

elevator_t elevator1, elevator2, elevator3;
char button[floors+floors]; //hidden outside button state
char button2[floors+floors]; //displayed outside button state
char old[floors+floors];
int waiting = 0;
int time = 0;
int ready1 = 0;
int ready2 = 0;
int serialFlag = 0;
char buffer[10];
int bufferindex = 0;

interrupt [TIM0_COMP] void timer0_compare(void)
{
    time = (time + 1)%50;
    if(UCSRA.7)
        serialFlag = 1;
    if(mode != sabbath)
        poll2();
    if(time)
    {
        return;
    }
    if (waiting)
    {
        if(elevator1->on)
        {
            elevator1->motorstate = (elevator1->motorstate + elevator1->traveldir+4)%4;

            if(elevator1->motorstate ==0)
                PORTA = 0x0a;
            else if(elevator1->motorstate==1)
                PORTA = 0x06;
            else if(elevator1->motorstate==2)
                PORTA = 0x05;
            else
                PORTA = 0x09;
            elevator1->count++;
        }
        elevator1->on = 0;
        if(elevator2->on)
        {
            elevator2->motorstate = (elevator2->motorstate + elevator2->traveldir+4)%4;

            if(elevator2->motorstate ==0)
                PORTB = 0x0a;
            else if(elevator2->motorstate==1)
                PORTB = 0x06;
            else if(elevator2->motorstate==2)
                PORTB = 0x05;
            else
                PORTB = 0x09;
            elevator2->count++;
        }
        elevator2->on = 0;
        if(elevator3->on)
        {
            elevator3->motorstate = (elevator3->motorstate + elevator3->traveldir+4)%4;

            if(elevator3->motorstate ==0)
                PORTC = 0x0a;
            else if(elevator3->motorstate==1)
                PORTC = 0x06;
            else if(elevator3->motorstate==2)
                PORTC = 0x05;
            else
                PORTC = 0x09;
            elevator3->count++;
        }
        elevator3->on = 0;

        waiting = 0;
    }
}

//poll the buttons
void poll()
{
    if(~PINA & 0x10)//Internal elevator buttons
        elevator1->button[0] = 1;
    if(~PINA & 0x20)
        elevator1->button[1] = 1;
    if(~PINA & 0x40)
        elevator1->button[2] = 1;
    if(~PINA & 0x80)
        elevator1->button[3] = 1;

    if(~PINB & 0x10)//Internal elevator buttons
        elevator2->button[0] = 1;
    if(~PINB & 0x20)
        elevator2->button[1] = 1;
    if(~PINB & 0x40)
        elevator2->button[2] = 1;
    if(~PINB & 0x80)
        elevator2->button[3] = 1;

    if(~PINC & 0x10)//Internal elevator buttons
        elevator3->button[0] = 1;
    if(~PINC & 0x20)
        elevator3->button[1] = 1;
    if(~PINC & 0x40)
        elevator3->button[2] = 1;
    if(~PINC & 0x80)
        elevator3->button[3] = 1;

    if(~PIND & 0x04 && !button2[1]){//Floor 0 up button
        button[1] = 1;
        button2[1] = 1;
        }
    if(~PIND & 0x08 && !button2[2]){//Floor 1 down button
        button[2] = 1;
        button2[2] = 1;
        }
    if(~PIND & 0x10 && !button2[3]){//Floor 1 up button
        button[3] = 1;
        button2[3] = 1;
        }
    if(~PIND & 0x20 && !button2[4]){//Floor 2 down button
        button[4] = 1;
        button2[4] = 1;
        }
    if(~PIND & 0x40 && !button2[5]){//Floor 2 up button
        button[5] = 1;
        button2[5] = 1;
        }
    if(~PIND & 0x80 && !button2[6]){//Floor 3 down button
        button[6] = 1;
        button2[6] = 1;
        }
}

void initialize(void);
void sendbuttons(elevator_t elevator)
{
     int i;
     for (i=0;i<floors;i++)
     {
          if(elevator->button[i] != elevator->oldbutton[i])
          {
               printf("inb%i%02i%i\r\n",elevator->id,i,elevator->button[i]);
               elevator->oldbutton[i] = elevator->button[i];
          }
     }

}
void main(void)
{
    int i;
    initialize();
    while(1)
    {
        if(mode != sabbath)
            poll();//poll the buttons

        for (i=1;i<floors + floors-1;i++)
        {
            if(button2[i]!=old[i])
                printf("outb%02i%i\r\n",i,button2[i]);
            old[i] = button2[i];
        }

        sendbuttons(elevator1);
        sendbuttons(elevator2);
        sendbuttons(elevator3);

        addRequests();//add requests to elevators

        oneStep(elevator1);//control elevators
        oneStep(elevator2);
        oneStep(elevator3);


        waiting = 1;

        while(waiting){}
    }
}


//**********************************************************
//Set it all up
void initialize(void)
{
    int i;
    //set up the ports

    DDRA=0x0f;    // lower 4 bits of PORT A are output, upper 4 are input
    DDRB=0x0f;    // lower 4 bits of PORT B are output, upper 4 are input
    DDRC=0x0f;    // lower 4 bits of PORT C are output, upper 4 are input
    DDRD=0x03;    // PORT D is an input

    PORTA = 0x0a;  //turn on stepper motor
    PORTB = 0x0a;  //turn on stepper motor
    PORTC = 0x0a;  //turn on stepper motor
    UCSRB = 0x18;
    UBRRL = 103;
    printf("init\r\n");
    elevator1 = (elevator_t)malloc(sizeof(Elevator));
    elevator2 = (elevator_t)malloc(sizeof(Elevator));
    elevator3 = (elevator_t)malloc(sizeof(Elevator));

    elevator1->floor = 0;
    elevator1->traveldir = 1;
    elevator1->turnaround = -1;
    elevator1->count = 0;
    elevator1->motorstate = 0;
    elevator1->on = 0;
    elevator1->paused = 0;
    elevator1->id = 1;

    for(i = 0; i<floors; i++)
    {
        elevator1->button[i] = 0;
        elevator1->uprequests[i] = 0;
        elevator1->downrequests[i] = 0;
        elevator1->oldbutton[i] = 0;
        elevator2->button[i] = 0;
        elevator2->uprequests[i] = 0;
        elevator2->downrequests[i] = 0;
        elevator2->oldbutton[i] = 0;
        elevator3->button[i] = 0;
        elevator3->uprequests[i] = 0;
        elevator3->downrequests[i] = 0;
        elevator3->oldbutton[i] = 0;

    }
    elevator2->floor = 0;
    elevator2->traveldir = 1;
    elevator2->turnaround = -1;
    elevator2->count = 0;
    elevator2->motorstate = 0;
    elevator2->on = 0;
    elevator2->paused = 0;
    elevator2->id = 2;

    elevator3->floor = 0;
    elevator3->traveldir = 1;
    elevator3->turnaround = -1;
    elevator3->count = 0;
    elevator3->motorstate = 0;
    elevator3->on = 0;
    elevator3->paused = 0;
    elevator3->id = 3;

    for(i = 0; i<floors+floors;i++)
    {
        button[i] = 0;
        button2[i] = 0;
        old[i] = 0;

    }

    //turn on interrupt 0 every 30ms? (controls elevator speed)
    TIMSK=2;      //turn on timer 0 cmp match ISR
    OCR0 = 250;   //set the compare re to 250 time ticks
    //prescalar to 64 and turn on clear-on-match
    TCCR0=0b00001011;

    //crank up the ISRs
    #asm
        sei
    #endasm

}

//decides where the elevator should turn around
int checkdirection(elevator_t elevator)
{
    int i;
    int flag = 0;
    for(i=elevator->floor;i<=floors-1 && i>=0;i = i + elevator->traveldir) //scan for requests in direction of travel
    {
        //there are pick up requests to go the same way in the direction of travel
        if((elevator->traveldir == 1 && elevator->uprequests[i]) || (elevator->traveldir == -1 && elevator->downrequests[i]))
        {
            //don't change direction, set turnaround floor to top/bottom floor
            if(elevator->traveldir == 1)
            {
                elevator->turnaround = floors-1;
            }
            else
            {
                elevator->turnaround = 0;
            }
            flag = 1;
            //elevator->on = 1;
            return 1;
        }
        //no pick up requests to go the same way, but drop off reqests in the direction of travel or pick up requests to go opposite way
        else if((elevator->traveldir == 1 && elevator->downrequests[i]) || (elevator->traveldir == -1 && elevator->uprequests[i]) || (elevator->button[i]))
        {


            //don't change direction yet, set the turnaround floor ((CHECK THIS LOGIC))
            if(elevator->button[i])
                elevator->turnaround = i; //highest floor with a drop off request
            if(elevator->traveldir == 1 && elevator->downrequests[i])
                elevator->turnaround = i; //highest floor that wants to go down
            else if(elevator->traveldir == -1 && elevator->uprequests[i])
                elevator->turnaround = i; //lowest floor that wants to go up

            flag = 1;
            if(elevator->floor != elevator->turnaround) //don't move if you need to turn on this floor
            {
                 //elevator->on = 1;
                 return 1;
            }
        }
    }
    //no more requests at all in direction of travel, check for other requests
     if(!flag)
     {
          for(i=elevator->floor-elevator->traveldir; i<=floors-1 && i>=0; i = i - elevator->traveldir)
          {
               if(elevator->button[i] || elevator->uprequests[i] || elevator->downrequests[i]) //request on a floor above/below
               {
                    elevator->turnaround = elevator->floor; //turn around now!
                    return 0;
               }
          }
          //don't change direction if we get out of that loop
          if(elevator->traveldir == 1)
          {
                elevator->turnaround = floors-1;
          }
          else
          {
              elevator->turnaround = 0;
          }
      }
      return 0;
}




//Function looks though current button requests and determines where to send elevators
//Each elevator follows SCAN algorithm on requests it is given
//Ideally, function resets button variables when it assigns requests to elevators.
//This function is responsible to only send elevator to floor if heading in correct direction
//(in terms of picking up passengers, dropping them off doesn't matter)
void addRequests()
{
    //Just send 1 elevator to do everything!
    if(mode==trivial)
    {
        algorithm1(elevator1);
    }
    else if(mode==simple)
    {
        algorithm1(elevator1);
        algorithm1(elevator2);
        algorithm1(elevator3);
    }
    else if(mode == normal)
    {
        algorithm2();
    }
    else if(mode == sabbath)
    {
        if(elevator3->floor == (floors+floors-2)/3  && !elevator3->paused)
            ready2 = 1;
        if(elevator2->floor == (floors+floors-2)/3  && !elevator2->paused)
            ready1 = 1;
        if(ready1)
            algorithm3(elevator1);
        if(ready2)
            algorithm3(elevator2);
        algorithm3(elevator3);
    }
}

void oneStep(elevator_t elevator) //main state machine
{
    int x,i, flag = 0;
    if(elevator->paused)
    {
        if((elevator->uprequests[elevator->floor] && elevator->traveldir == 1) ||
           (elevator->downrequests[elevator->floor] && elevator->traveldir == -1) || elevator->button[elevator->floor])
        {
            i = elevator->floor;
            //printf("Elevator #%i Doors remaining open (button held down)\r\n",elevator->id);
            elevator->paused = pause;
            //request satisfied
            if(elevator->traveldir == 1) //this could be wrong
            {
                        elevator->uprequests[i] = 0;
                  }
                  else
                  {
                        elevator->downrequests[i] = 0;
                  }
            button2[((i<<2) + 1 + elevator->traveldir)>>1] = 0;
            button[((i<<2) + 1 + elevator->traveldir)>>1] = 0;
            elevator->button[elevator->floor] = 0;
        }

        elevator->paused--;
        if(elevator->paused==0)
        {
            //printf("Elevator #%i Doors closing\r\n",elevator->id);
            printf("closing%i\r\n",elevator->id);
        }
    }
    else
    {
        //just hit a floor or idling on floor
        if(elevator->count==floorsize || elevator->count == 0)
        {
            if(elevator->count == 0) //idling on a floor
            {
                if(elevator->button[elevator->floor])
                {
                    flag = 1;
                    elevator->button[elevator->floor] = 0;
                }
            }
            //just hit a floor, update floor number and display it
            if(elevator->count==floorsize)
            {
                elevator->floor = elevator->floor + elevator->traveldir;
                //if we are going to stop at this floor, update button
                //before calling checkdirection
                if(elevator->button[elevator->floor])
                {
                    flag = 1;
                    elevator->button[elevator->floor] = 0;
                }
                checkdirection(elevator);
                if(elevator->floor==elevator->turnaround)
                    elevator->traveldir = 0 - elevator->traveldir;

                printf("arrived%i%02i\r\n",elevator->id,elevator->floor);
                elevator->count = 0;
            }
            //passenger wants to get off on this floor, so elevator needs to stop
            if(flag)
            {
                printf("opening%i%i\r\n",elevator->id,elevator->traveldir+1);
                elevator->paused = pause;
                //requests to get on in same direction satisfied
                if(elevator->floor == elevator->turnaround) //elevator is turning around here, can satisfy other elevator requests
                {
                  if(elevator->traveldir == 1)
                  {
                      elevator1->downrequests[elevator->floor] = 0;
                      elevator2->downrequests[elevator->floor] = 0;
                      elevator3->downrequests[elevator->floor] = 0;
                  }
                  else
                  {
                      elevator1->uprequests[elevator->floor] = 0;
                      elevator2->uprequests[elevator->floor] = 0;
                      elevator3->uprequests[elevator->floor] = 0;
                  }
                  x = ((elevator->floor<<2) + 1 - elevator->traveldir)>>1;
                }
                else //elevator is continuing on, can satisfy other elevator requests
                {
                    if(elevator->traveldir == 1)
                    {
                        elevator1->uprequests[elevator->floor] = 0;
                        elevator2->uprequests[elevator->floor] = 0;
                        elevator3->uprequests[elevator->floor] = 0;
                    }
                    else
                    {
                        elevator1->downrequests[elevator->floor] = 0;
                        elevator2->downrequests[elevator->floor] = 0;
                        elevator3->downrequests[elevator->floor] = 0;
                    }

                    //turn off outside button
                    x = ((elevator->floor<<2) + 1 + elevator->traveldir)>>1;
                }
                button2[x] = 0;
            }
            //there is an outside request in the direction of travel, open to pick them up
            if((elevator->traveldir == 1 && elevator->uprequests[elevator->floor]) || (elevator->traveldir == -1 && elevator->downrequests[elevator->floor]) )
            {
                //printf("Elevator #%i doors opening to pick someone up\r\n",elevator->id);
                printf("opening%i%i\r\n",elevator->id,elevator->traveldir+1);
                elevator->paused = pause;
                //request to pick up on this floor satisfied
                if(elevator->traveldir == 1)
                {
                    elevator->uprequests[elevator->floor] = 0;
                }
                else
                {
                    elevator->downrequests[elevator->floor] = 0;
                }

                //turn  off outside button
                x = ((elevator->floor<<2) + 1 + elevator->traveldir)>>1;
                button2[x] = 0;
                button[x] = 0;
            }
            if(!elevator->paused)
            {
                x = checkdirection(elevator); //we need to check again after new passengers have gotten on.
                if(elevator->floor == elevator->turnaround) //this is where we turn around
                {
                    elevator->traveldir = 0 - elevator->traveldir; //switch directions
                    //printf("Switched directions to %i\r\n",elevator->traveldir);
                }
                if(x)
                    elevator->on = 1;
            }
        }
        //don't turn around mid-floor
        else
        {
            elevator->on = 1;
        }
    }
}

int hasrequests(elevator_t elevator, int requestfloor, int requestdirection)//checks if an elevator has requests and returns 200*numrequests
{
    int i, sum = 0;
    if(elevator->paused && elevator->floor != requestfloor) //add distance if elevator is stopped on another floor
        sum = sum + 200;
    for(i=0; i<floors; i++)
    {
        if(elevator->downrequests[i]||elevator->uprequests[i]||elevator->button[i])
        {
            //add distance if elevator has a request on another floor or in the opposite direction
            if(i != requestfloor || (elevator->uprequests[i] && requestdirection == -1) || (elevator->downrequests[i] && requestdirection == 1))
                sum = sum + 200;
        }
    }
    return sum;

}


int distance(elevator_t elevator, int requestfloor, int requestdirection)
{
    int elevatorfloor, requests;
    requests = hasrequests(elevator,requestfloor,requestdirection);
    elevatorfloor = elevator->floor;
    if(!requests)
        return abs(elevator->floor - requestfloor);
    //if elevator is moving then its floor must be updated to the next floor it will pass
    if(!elevator->paused)
      elevatorfloor = elevatorfloor + elevator->traveldir;
    //worst and best cases, elevator heading in same direction as request
    if(elevator->traveldir==requestdirection)
    {
        //best case, elevator on way to request
        if((elevator->traveldir==1 && elevatorfloor<=requestfloor)||(elevator->traveldir==-1 && elevatorfloor>=requestfloor))
        {
            return requests + abs(requestfloor - elevatorfloor);
        }
        //worst case, elevator needs to switch direction twice
        else
        {
            return requests + floors+floors - abs(requestfloor - elevatorfloor);
        }
    }
    //elevator needs to switch direction once
    else
    {
        //elevator heading to top floor
        if(elevator->traveldir==1)
        {
            return requests + (floors-1-elevatorfloor)+(floors-1-requestfloor);
        }
        //elevator heading to bottom floor
        else
        {
            return requests + elevatorfloor+requestfloor;
        }
    }

}

void algorithm1(elevator_t elevator) //for trivial and simple modes
{
    int i;
    for(i=1;i<floors+floors-1;i++) //add all outside requests to elevator 1 for now
    {
        if(button[i]) //button is pressed
        {
          if(i%2 == 1) //this is an up button
          {
            elevator->uprequests[i>>1] = 1;
          }
          else //this is a down button
          {
           elevator->downrequests[i>>1] = 1;
          }
          if(mode!=trivial && elevator->id==3)
            button[i] = 0; //button turns off, request received

        }
    }
}

void algorithm2() //for normal mode
{
    int i;
    int d1;
    int d2;
    int d3;
    elevator_t choice;
    for(i=1;i<floors+floors-1;i++) //loop through outside buttons
    {
        if(button[i]) //button is pressed
        {
            //if elevator is already open on this floor, give request to that elevator
            if(elevator1->paused && elevator1->floor == (i>>1) && elevator1->traveldir==i%2)
            {
              if(i%2 == 1) //up button
                elevator1->uprequests[elevator1->floor] = 1;
              else //down button
                elevator1->downrequests[elevator1->floor] = 1;
            }
            else if(elevator2->paused && elevator2->floor == (i>>1) && elevator2->traveldir==i%2)
            {
              if(i%2 == 1) //up button
                elevator2->uprequests[elevator2->floor] = 1;
              else //down button
                elevator2->downrequests[elevator2->floor] = 1;
            }
            else if(elevator3->paused && elevator3->floor == (i>>1) && elevator3->traveldir==i%2)
            {
              if(i%2 == 1) //up button
                elevator3->uprequests[elevator3->floor] = 1;
              else //down button
                elevator3->downrequests[elevator3->floor] = 1;
            }
            //otherwise, calculate distances and compare
            else
            {
                if(i%2 == 1) //this is an up button
                {
                    d1 = distance(elevator1,i>>1,1);
                    d2 = distance(elevator2,i>>1,1);
                    d3 = distance(elevator3,i>>1,1);
                }
                else //this is a down button
                {
                    d1 = distance(elevator1,i>>1,-1);
                    d2 = distance(elevator2,i>>1,-1);
                    d3 = distance(elevator3,i>>1,-1);
                }
                if(d1<=d2 && d1<=d3)
                    choice = elevator1;
                else if(d2<=d3)
                    choice = elevator2;
                else
                    choice = elevator3;

                if(i%2 == 1) //this is an up button
                {
                    choice->uprequests[i>>1] = 1;
                }
                else //this is a down button
                {
                    choice->downrequests[i>>1] = 1;
                }
            }
            button[i] = 0;
        }
    }
}

void algorithm3(elevator_t elevator) //for sabbath mode
{
    int i;
    for(i = 0; i< floors; i++)
    {
        if((elevator->floor != i))
        {
            if(i>0)
                elevator->downrequests[i] = 1;
            if(i<floors-1)
                elevator->uprequests[i] = 1;
        }

    }
}

void poll2()
{
    int x;
    elevator_t elevator = NULL;
    if(!serialFlag)
        return;
    serialFlag = 0;
    buffer[bufferindex] = getchar();
    if(buffer[bufferindex]=='z')
    {
        if(bufferindex>=3 && buffer[bufferindex-3]=='o')
        {
            x = ((int)buffer[bufferindex-2]-48)*10+(int)buffer[bufferindex-1]-48;
            if (x>0 && x<2*floors-1 && !button2[x])
            {
                button[x] = 1;
                button2[x] = 1;
            }
        }
        else if(bufferindex>=4 && buffer[bufferindex-4]=='i')
        {
            x = ((int)buffer[bufferindex-2]-48)*10+(int)buffer[bufferindex-1]-48;
            if(buffer[bufferindex-3]=='1')
                elevator = elevator1;
            else if(buffer[bufferindex-3]=='2')
                elevator = elevator2;
            else if(buffer[bufferindex-3]=='3')
                elevator = elevator3;

            if(elevator != NULL && x>=0 && x< floors)
            {
                elevator->button[x] = 1;
            }
        }
        bufferindex=0;
    }
    else
        bufferindex++;

    if(bufferindex>=8)
        bufferindex = 0;
}