// -------- Include files ------------

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include "binary.h"

// -------- constant and function decerations ------------

#define OUTPUT  1
#define INPUT   0

#define forward  1
#define backward 0

#define BAUD                 (long)4800      /* baudrate for serial interface */
#define FREQ                 (long)16000000	 /* oscillator-frequency in Hz */
#define UART_RATE(B, F)      ((F/(B*8))-1)



// -------- custom data types ------------

typedef struct stepper4ph_t {      
      unsigned Port       :3;
      unsigned MotorNum   :1;
      unsigned State      :3;
      unsigned Direction  :1;
} stepper4ph ;

enum Commands {
    IDLE, FORWARD, BACKWARD, LEFT, RIGHT, OPEN, CLOSE, SPEED, LED, EXPLODE
};

enum RX_Modes { RX_COMMAND, RX_NUMBER };

// -------- global variables ------------

/* first  column is the index of the next state, 
   second column is the data for that state */
  unsigned char stateTableBackward[] = {  
      (3)+(b0001<<4),                    
      (0)+(b0010<<4),
      (1)+(b0100<<4),
      (2)+(b1000<<4)
   };  
   
  unsigned char stateTableForward[] = {  
      (1)+(b0001<<4),                    
      (2)+(b0010<<4),
      (3)+(b0100<<4),
      (0)+(b1000<<4)
   }; 

  uint16_t               ibuff;     // input command buffer
  unsigned char          rx_mode;   // inputting a command or a number
  unsigned char          temp_command;
  
  
  volatile unsigned char command;  // command as updated by irc routine
  volatile unsigned char argument;
  
  stepper4ph motor1, motor2, motor3; // "make a motor"

// -------- function declerations ------------

void stepmotor (stepper4ph * motor);
void Delay(int delay);
void inline changeIO(char port, char mask, char bits);
void serial_init();
void flash( unsigned char port, unsigned char bits, unsigned char flashes);
void decode_command();
void decode_number();
void drive (unsigned int dist);
void grip  (unsigned int amount);


// -------- some real, actual code ------------

int main () {

    // set up directions 
    DDRA = (INPUT << PA0 | INPUT << PA1 |INPUT << PA2 |INPUT << PA3 |INPUT << PA4 |INPUT << PA5 |INPUT << PA6 |INPUT << PA7);
    DDRB = (OUTPUT << PB0 | OUTPUT << PB1 |OUTPUT << PB2 |OUTPUT << PB3 |OUTPUT << PB4 |OUTPUT << PB5 |OUTPUT << PB6 |OUTPUT << PB7);
    DDRC = (OUTPUT << PC0 | OUTPUT << PC1 |OUTPUT << PC2 |OUTPUT << PC3 |INPUT << PC4 |INPUT << PC5 |INPUT << PC6 |INPUT << PC7);
    DDRD = (INPUT << PD0 | INPUT << PD1 |INPUT << PD2 |INPUT << PD3 |INPUT << PD4 |INPUT << PD5 |INPUT << PD6 |INPUT << PD7);

    motor1.Port     = 1; // 0 = portA, so this is port C
    motor1.MotorNum = 0; // 0 is bits 0-3, 1 is bits 4-7
    motor1.Direction = forward;

    motor2.Port     = 1;
    motor2.MotorNum = 1;
    motor2.Direction = backward;
  
    motor3.Port      = 2;
    motor3.MotorNum  = 0;     
  
    serial_init();
    sei();  //enable innterupts
  
    command = IDLE;
    rx_mode = RX_COMMAND;
  
   
   
   while(1){
     switch (command) {
     
       case FORWARD:
         //flash( 2, ~b00000001, argument);
         motor2.Direction = backward;
         motor1.Direction = forward;
         drive(argument*4);
         command = IDLE;
       break;
       
       case BACKWARD:
         //flash( 2, ~b00000010, argument); 
         motor2.Direction = forward;
         motor1.Direction = backward;
         drive(argument*4);
         command = IDLE;
       break;
       
       case LEFT:
         //flash( 2, ~b00000100, argument);
         motor2.Direction = forward;
         motor1.Direction = forward;
         drive(argument*4);
         command = IDLE;
       break;
       
       case OPEN:
         motor3.Direction = backward;
         grip(argument);
         command = IDLE;
       break;
        
       case CLOSE:
         motor3.Direction = forward;
         grip(argument);
         command = IDLE;
       break;
       
       case SPEED:
          command = IDLE;
       break;
       
       case LED:
          if (argument == 0) {
            changeIO(3, b01000000, b00000000);
          } else {          
            changeIO(3, b01000000, b01000000);
          }
          command = IDLE;
       break;
       
       case RIGHT:
         motor2.Direction = backward;
         motor1.Direction = backward;
         drive(argument*4);
         command = IDLE;
       break;
     
       case IDLE:
        // command = IDLE;
       break;
     
     }          
   }

}

//--------------------------------| FUNCTIONS |-------------------------
void drive (unsigned int dist) {

   unsigned int x;

   for (x = dist; x != 0; x--) {   
     stepmotor(&motor1);
     stepmotor(&motor2);
     Delay(50000);   
   }
}

void grip (unsigned int amount) {

   unsigned int x;

   for (x = amount; x != 0; x--) {   
     stepmotor(&motor3);
     Delay(55000);   
   }
}

void flash( unsigned char port, unsigned char bits, unsigned char flashes) {

  unsigned char blink;
  
  for (blink = flashes; blink != 0; blink--) {
     changeIO(port, bits, 0xFF);
     Delay(65000);
     Delay(65000);
     Delay(65000);
     Delay(65000);
          Delay(65000);
     Delay(65000);
     Delay(65000);
     Delay(65000);
     changeIO(port, bits, 0x00);
     Delay(65000);
     Delay(65000);
     Delay(65000);
     Delay(65000);
          Delay(65000);
     Delay(65000);
     Delay(65000);
     Delay(65000);
  }

}

void serial_init() {
	
	UBRRH = (unsigned char)(UART_RATE(BAUD, FREQ) >> 8);
    UBRRL = (unsigned char) UART_RATE(BAUD, FREQ);
      
   /* set 2X baud rate */
   UCSRA = (1<<U2X);
   
   /* Enable receiver and transmitter; enable RX interrupt */
	UCSRB = (1 << RXEN) | (1 << TXEN) | (1 << RXCIE);
   
   /* Set frame format: No parity, 8data, 1stop bit */
   UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);   //For devices without Extended IO

}

// INTERRUPT can be interrupted
// SIGNAL can't be interrupted
SIGNAL (SIG_UART_RECV) {
  
  unsigned char temp;
  
  temp = UDR;
  
  if (temp == '\r') {
    // dispose of stupid dos characters
  } else if (temp == ' ') { // switch to number mode, reset number
    rx_mode = RX_NUMBER;
  } else if (temp == '\n') { // switch to command mode, submit command if valie 
    rx_mode = RX_COMMAND;
    command = temp_command; // submitt command
  } else {  
     ibuff <<= 8;
     ibuff |= temp ;

     if (rx_mode == RX_NUMBER) {       
       decode_number();
     } else if (rx_mode == RX_COMMAND) {       
       decode_command();
     }

  }
}

#define isdigit(c)  (('0' <= (c)) && ((c) <= '9'))

void decode_number() {
  if ((isdigit(ibuff & 0x00FF)) && 
      (isdigit( (ibuff & 0xFF00) >> 8) ) 
     ){
    argument = (ibuff & 0x0F) + ((ibuff & 0x0F00) >> 8)*10;
  }  
}

void decode_command() {

        switch(ibuff) {
      
        case 'ri':
          temp_command = RIGHT; 
        break;
        
        case 'le':
          temp_command = LEFT;              
        break;
        
        case 'ba':
          temp_command = BACKWARD;   
        break;
        
        case 'fo':
          temp_command = FORWARD;   
        break;    
        
        case 'op':
          temp_command = OPEN;
        break;
        
        case 'cl':
          temp_command = CLOSE;
        break;
        
        case 'ld':
          temp_command = LED;
        break;
        
        case 'id':
          temp_command = IDLE;    
        break;
      }
}

void stepmotor (stepper4ph * motor) {
          
   unsigned const char *  stateTable;
   unsigned char temp, mask;
   
   stateTable = stateTableBackward;                      //choose state table
   if (motor->Direction == forward) {
      stateTable = stateTableForward;
   } 
   
   motor->State  = (*(stateTable + motor->State) & 0x0F); // execute state machine
   temp =  (*(stateTable + motor->State) & 0xF0);
   
   mask = 0x0F;                                           // choose bit positions
   if (motor->MotorNum == 0) { temp >>= 4; mask = 0xF0; }
  
   changeIO( motor->Port, mask, temp);                    // change io bits
  
  }

// mask is the bits you dont want to change
void inline changeIO(char port, char mask, char bits) {
 switch (port) {
     case 0:
        PORTA = (PORTA&mask)|(bits&~mask);
        break;
     case 1:
        PORTB = (PORTB&mask)|(bits&~mask);
        break;
     case 2:
        PORTC = (PORTC&mask)|(bits&~mask);
        break;
     case 3:
        PORTD = (PORTD&mask)|(bits&~mask);
        break;
    }
}

void Delay(int delay) {
  int x;
  for (x = delay; x != 0; x--) {
    asm volatile ("nop"::); 
  }
}