#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>

#define SENSOR_PORT  PINF

#define ON     1
#define OFF     0
#define CW     1
#define CCW     0

#define MOTOR_PORT   PORTB
#define PWM_PORT  0x00
#define PWM1_ON   (MOTOR_PORT|=0x01)
#define PWM1_OFF  (MOTOR_PORT&=0xFE)
#define DIR1_ON   (MOTOR_PORT|=0x02)
#define DIR1_OFF   (MOTOR_PORT&=0xFD)
#define ENABLE1_OFF  (MOTOR_PORT|=0x04)
#define ENABLE1_ON  (MOTOR_PORT&=0xFB)
#define BREAK1_ON  (MOTOR_PORT|=0x08)
#define BREAK1_OFF   (MOTOR_PORT&=0xF7)

#define PWM2_ON   (MOTOR_PORT|=0x10)
#define PWM2_OFF  (MOTOR_PORT&=0xEF)
#define DIR2_ON   (MOTOR_PORT|=0x20)
#define DIR2_OFF   (MOTOR_PORT&=0xDF)
#define ENABLE2_OFF  (MOTOR_PORT|=0x40)
#define ENABLE2_ON  (MOTOR_PORT&=0xBF)
#define BREAK2_ON  (MOTOR_PORT|=0x80)
#define BREAK2_OFF   (MOTOR_PORT&=0x7F)

void port_init(void)
{
 PORTA = 0x00;
 DDRA  = 0x00;
 PORTB = 0x00;
 DDRB  = 0xff;
 PORTC = 0x00; //m103 output only
 DDRC  = 0xff;
 PORTD = 0x00;
 DDRD  = 0x00;
 PORTE = 0x00;
 DDRE  = 0x00;
 PORTF = 0x00;
 DDRF  = 0x00;
 PORTG = 0x00;
 DDRG  = 0x03;
}

// UART0 을 이용한 출력
void tx0Char(char message)
{
 while (((UCSR0A>>UDRE0)&0x01) == 0) ;  // UDRE, data register empty       
    UDR0 = message;
}

static int Putchar(char c, FILE *stream)
{
 tx0Char(c);
    return 0;
     
}

//UART0 initialize
// desired baud rate: 9600
// actual: baud rate:9615 (0.2%)
// char size: 8 bit
// parity: Disabled
void uart0_init(void)
{
 UCSR0B = 0x00; //disable while setting baud rate
 UCSR0A = 0x00;
 UCSR0C = 0x06;
 UBRR0L = 0x67; //set baud rate lo
 UBRR0H = 0x00; //set baud rate hi
 UCSR0B = 0x18;
}

//call this routine to initialize all peripherals
void init_devices(void)
{
 //stop errant interrupts until set up
 cli(); //disable all interrupts
 XMCRA = 0x00; //external memory
 XMCRB = 0x00; //external memory
 port_init();
 uart0_init();
 fdevopen(Putchar,0); 
 
 MCUCR = 0x00;
 EICRA = 0x00; //extended ext ints
 EICRB = 0x00; //extended ext ints
 EIMSK = 0x00;
 TIMSK = 0x00; //timer interrupt sources
 ETIMSK = 0x00; //extended timer interrupt sources
 
 sei(); //re-enable interrupts
 //all peripherals are now initialized
}

void delay(int n)
{
 volatile int i,j;
 for(i=1;i<n;i++)
 {
     for(j=1;j<600;j++);
 }
}

void delay2(int n)
{
 volatile int i,j;
 for(i=1;i<n;i++)
 {
     for(j=1;j<10;j++);
 }
}

void M1A(int onoff){
 if(onoff==ON)
  MOTOR_PORT = MOTOR_PORT|0x01;
 else
  MOTOR_PORT = MOTOR_PORT&0xFE; 
}

void M1A_(int onoff){
 if(onoff==ON)
  MOTOR_PORT = MOTOR_PORT|0x02;
 else
  MOTOR_PORT = MOTOR_PORT&0xFD; 
}

void M1B(int onoff){
 if(onoff==ON)
  MOTOR_PORT = MOTOR_PORT|0x04;
 else
  MOTOR_PORT = MOTOR_PORT&0xFB; 
}

void M1B_(int onoff){
 if(onoff==ON)
  MOTOR_PORT = MOTOR_PORT|0x08;
 else
  MOTOR_PORT = MOTOR_PORT&0xF7; 
}

void Motor1(int CWCCW){
 if(CWCCW==CW){
  MOTOR_PORT|=0x01;//MOTOR1_ON
 MOTOR_PORT|=0x02;//DIR1_CW
 }else{
 MOTOR_PORT|=0x01;//MOTOR1_ON
 MOTOR_PORT&=0xFD;//DIR1_CCW
 }
}

void Motor2(int CWCCW){
 if(CWCCW==CW){
  MOTOR_PORT|=0x10;//MOTOR2_ON
 MOTOR_PORT|=0x20;//DIR2_CW
 }else{
 MOTOR_PORT|=0x10;//MOTOR2_ON
 MOTOR_PORT&=0xDF;//DIR2_CCW
 }
}

void MOTORSTOP(void){
  M1A(OFF);
 M1A_(OFF);
  M1B(OFF);
 M1B_(OFF);
 
}

//
int main(void)
{
 unsigned char sensor;
// volatile unsigned char stepleft=0, stepright=0;
 

 init_devices();

 
                        MOTOR_PORT&=0xFB; //ENABLE1_ON
                        MOTOR_PORT&=0xF7; //BREAK1_OFF

                        MOTOR_PORT&=0xBF; //ENABLE2_ON
                        MOTOR_PORT&=0x7F; //BREAK2_OFF

       MOTORSTOP();
      delay(2000);

 
 while(1){
     sensor = SENSOR_PORT & 0x0F;
  printf("\r\n Sensor:%x    ",sensor);
  switch(sensor){
 case 0x0c:
 case 0x08:  printf("맨오른쪽 \r\n");
       PORTG= 0x01;
      Motor1(0);
      Motor2(0);
      delay(10);
      MOTORSTOP();
      delay(1);
      break;
 case 0x04:  printf("오른쪽 \r\n");   
      PORTG= 0x02;
      Motor1(0);
      Motor2(0);
      delay(10);
      MOTORSTOP();
      delay(1);      
      break;
 case 0x03:
 case 0x02:  printf("왼쪽 \r\n");
        PORTG= 0x03;      
      Motor1(1);
      Motor2(1);
      delay(10);
      MOTORSTOP();
      delay(1);
      break;
 case 0x01:  printf("맨왼쪽\r\n");
      PORTG= 0x00;
      Motor1(1);
      Motor2(1);
      delay(10);
      MOTORSTOP();
      delay(1);
      break;
 case 0x0f:
 case 0x00:  printf("All White");
      MOTOR_PORT = 0x31;
        //Motor1(1);Motor2(0);
      delay(10);
      //MOTORSTOP();
      MOTOR_PORT = 0x44;
      delay(1);
      break;   
 
  }  
 }
 return 0;
}