// CONFIG
#pragma config FOSC = INTOSCIO  // Oscillator Selection bits (INTOSC oscillator: I/O function on RA6/OSC2/CLKOUT pin, I/O function on RA7/OSC1/CLKIN)
#pragma config WDTE = OFF       // Watchdog Timer Enable bit (WDT disabled)
#pragma config PWRTE = OFF      // Power-up Timer Enable bit (PWRT disabled)
#pragma config MCLRE = ON       // RA5/MCLR/VPP Pin Function Select bit (RA5/MCLR/VPP pin function is MCLR)
#pragma config BOREN = ON       // Brown-out Detect Enable bit (BOD enabled)
#pragma config LVP = OFF        // Low-Voltage Programming Enable bit (RB4/PGM pin has digital I/O function, HV on MCLR must be used for programming)
#pragma config CPD = OFF        // Data EE Memory Code Protection bit (Data memory code protection off)
#pragma config CP = OFF         // Flash Program Memory Code Protection bit (Code protection off)
#define _XTAL_FREQ 4000000L

#include <xc.h>
#include <pic16f628a.h>

#define _BIT(B,b,v) B = (B & ~(1<<b)) | (v<<b)
#define _GET(B,b) ((B>>b)&1)

#define CLOSE 1
#define OPEN  0
#define DELAY_CLOSING 2000 //ms
#define TIME_BLINK 500 //ms
#define TICK_PER_TURN 100 //number of ticks in a loop
#define TURNS ((((_XTAL_FREQ/4)*(TIME_BLINK))/1000)/(TICK_PER_TURN))

//PORTB
#define BTN 5
#define END 4
#define EN3 0
#define EN4 1
#define CH3 3
#define CH4 2 //1>close
//PORTA
#define LED 0

void init_IO(){
    TRISA = 0b11111110;
    TRISB = 0b11110000;
    OPTION_REG &= 0x7F; //Pullup on B
    
    PORTA = 0x00;
    PORTB = 0x00;
}

void move(char dir){
    _BIT(PORTB, CH4, dir); //Change motor direction
    _BIT(PORTB, EN4, 1); //Enable motor
    
    if(dir == OPEN){
        while(_GET(PORTB, END) == 0); //While closed
    }
    else{ //Close
        while(_GET(PORTB, END) != 0); //While not in tray
        __delay_ms(DELAY_CLOSING);
    }
    
    _BIT(PORTB, EN4, 0); //Disable motor
}

void open_blink()
{
    char i;
    char pass1=1, pass2=0;
    long timer;
    
    _BIT(PORTA, LED, 1);
    while(_GET(PORTB, BTN) == 0); //Wait until unpress
    __delay_ms(500); //debounce
    
    for(i=0; i<3; i++){
        _BIT(PORTA, LED, 0);
        for(timer=0; timer<TURNS; timer++){
            if(_GET(PORTB, BTN) == 0)
                pass1 = 0;
        }
        
        _BIT(PORTA, LED, 1);
        for(timer=0; timer<TURNS; timer++){
            if(_GET(PORTB, BTN) == 0){
                if(i!=2)
                    pass1 = 0;
                else
                    pass2 = 1;
            }
        }
    }
    
    if((_GET(PORTB, BTN) != 0) && (pass1==1) && (pass2==1))
        move(OPEN);
}

void test_motor(){
    _BIT(PORTB, CH4, 1);
    _BIT(PORTA, LED, 1);
    __delay_ms(1000);

    _BIT(PORTB, CH4, 0);
    _BIT(PORTA, LED, 0);
    __delay_ms(1000);
}

void main(void) {
    init_IO();
    
    while(1){
        
        if(_GET(PORTB, BTN) == 0) //Button pressed
        {
            _BIT(PORTA, LED, 1);
            
            if(_GET(PORTB, END) == 0) //closed 
                open_blink();
            else
                move(CLOSE);
            
            _BIT(PORTA, LED, 0);
        }
    
    }
    return;
}