#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <util/delay.h>
/* RFM12B INTERFACE */
#define SCK 5 // SPI clock
#define SDO 4 // SPI Data output (RFM12B side)
#define SDI 3 // SPI Data input (RFM12B side)
#define CS 2 // SPI SS (chip select)
#define NIRQ 2 // (PORTD)
/* IO CONTROL */
#define HI(x) PORTB |= (1<<(x))
#define LO(x) PORTB &= ~(1<<(x))
#define WAIT_NIRQ_LOW() while(PIND&(1<<NIRQ))
/* LED */
#define LED 0
#define LED_OFF() PORTD &= ~(1<<LED)
#define LED_ON() PORTD |= (1<<LED)
#define BAUDRATE 25 // 19200 at 8MHz

void delay_ms(uint16_t ms) 
{
    while ( ms ) 
    {
        _delay_ms(1);
        ms--;
    }
}

void portInit() {
	HI(CS);
	HI(SDI);
	LO(SCK);

	DDRB = (1<<CS) | (1<<SDI) | (1<<SCK) | (1<<PB0) | (1<<PB1);
	DDRD = (1<<LED) | (1<<PD7)| (1<<PD6) | (1<<PD5) | (1<<PD1); //set output pins

	PORTB &= ~((1<<PB0) | (1<<PB1));
	PORTD &= ~((1<<PD7) | (1<<PD6) | (1<<PD5));
}

unsigned int writeCmd(unsigned int cmd) {
	unsigned char i;
	unsigned int recv;
	recv = 0;
	LO(SCK);
	LO(CS);
	for(i=0; i<16; i++){
		if(cmd&0x8000) HI(SDI); else LO(SDI);
		HI(SCK);
		recv<<=1;
		if( PINB&(1<<SDO) ){
			recv|=0x0001;
		}
		LO(SCK);
		cmd<<=1;
	}
	HI(CS);
	return recv;
}

void rfInit() {
	writeCmd(0x80D7); //EL,EF,433band,12.0pF
	writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC (bug was here)
	writeCmd(0xA640); //freq select
	writeCmd(0xC647); //4.8kbps
	writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
	writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
	writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR (FIFO level = 8)
	writeCmd(0xCED4); //SYNC=2DD4;
	writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
	writeCmd(0x9850); //!mp,90kHz,MAX OUT
	writeCmd(0xCC17); //!OB1,!OB0, LPX,!ddy,DDIT,BW0
	writeCmd(0xE000); //NOT USE
	writeCmd(0xC800); //NOT USE
	writeCmd(0xC040); //1.66MHz,2.2V
}

unsigned char rfRecv() {
	unsigned int data;
	while(1) {
		data = writeCmd(0x0000);
		if ( (data&0x8000) ) {
			data = writeCmd(0xB000);
			return (data&0x00FF);
		}
	}
}

void FIFOReset() {
	writeCmd(0xCA81);
	writeCmd(0xCA83);
}

int stepcount=0;

void Turn(uint8_t dir)
{
	if(dir==1 && stepcount<30)//left
	{
		if(stepcount % 4 == 0){
			PORTD |= (1<<PD6); 
			delay_ms(10);
			PORTD &= ~(1<<PD6);  
		}
	   
		if(stepcount % 4 == 1 || stepcount % 4 == -3){
			PORTB |= (1<<PB0);
			delay_ms(10);
			PORTB &= ~(1<<PB0);  
		}

		if(stepcount % 4 == 2 || stepcount % 4 == -2){
			PORTD |= (1<<PD7); 
			delay_ms(10);
			PORTD &= ~(1<<PD7); 
		}

		 if(stepcount % 4 == 3 || stepcount % 4 == -1){
			PORTD |= (1<<PD5);
			delay_ms(10);
			PORTD &= ~(1<<PD5); 
		}
		stepcount++;
	}
	
	if(dir==2 && stepcount > -30)//right
	{
		
		if(stepcount % 4 == 3 || stepcount % 4 == -1){
			PORTD |= (1<<PD5); 
			delay_ms(10);
			PORTD &= ~(1<<PD5);  
		}
	   
		if(stepcount % 4 == 2 || stepcount % 4 == -2){
			PORTD |= (1<<PD7);
			delay_ms(10);
			PORTD &= ~(1<<PD7);  
		}

		
		if(stepcount % 4 == 1 || stepcount % 4 == -3){
			PORTB |= (1<<PB0); 
			delay_ms(10);
			PORTB &= ~(1<<PB0); 
		}

		if(stepcount % 4 == 0){ 
			PORTD |= (1<<PD6);
			delay_ms(10);
			PORTD &= ~(1<<PD6); 
		}
		stepcount--;
	}
	
	if(dir==0 && stepcount > 0)
	{
		if(stepcount % 4 == 3 || stepcount % 4 == -1){
			PORTD |= (1<<PD5); 
			delay_ms(10);
			PORTD &= ~(1<<PD5);  
		}
	   
		if(stepcount % 4 == 2 || stepcount % 4 == -2){
			PORTD |= (1<<PD7);
			delay_ms(10);
			PORTD &= ~(1<<PD7);  
		}

		
		if(stepcount % 4 == 1 || stepcount % 4 == -3){
			PORTB |= (1<<PB0); 
			delay_ms(10);
			PORTB &= ~(1<<PB0); 
		}

		if(stepcount % 4 == 0){ 
			PORTD |= (1<<PD6);
			delay_ms(10);
			PORTD &= ~(1<<PD6); 
		}
		stepcount--;	
	}
	
	if(dir==0 && stepcount < 0)
	{
		if(stepcount % 4 == 0){
			PORTD |= (1<<PD6); 
			delay_ms(10);
			PORTD &= ~(1<<PD6);  
		}
	   
		if(stepcount % 4 == 1 || stepcount % 4 == -3){
			PORTB |= (1<<PB0);
			delay_ms(10);
			PORTB &= ~(1<<PB0);  
		}

		if(stepcount % 4 == 2 || stepcount % 4 == -2){
			PORTD |= (1<<PD7); 
			delay_ms(10);
			PORTD &= ~(1<<PD7); 
		}

		 if(stepcount % 4 == 3 || stepcount % 4 == -1){
			PORTD |= (1<<PD5);
			delay_ms(10);
			PORTD &= ~(1<<PD5); 
		}
		stepcount++;	
	}

}

void pwm_on_pb1()
{
    DDRB |= (1 << DDB1); // PB1 is now an output
    OCR1A = 0; //kitoltesi tenyezo
    TCCR1A |= (1 << COM1A1); // set none-inverting mode
    TCCR1A |= (1 << WGM12) | (0 << WGM11) | (1 << WGM10); // 8 bit fast pwm
	TCCR1B |= (1 << CS10); // prescaler = 1 
}

volatile uint8_t count = 0;

int main(void) {
	int i=0;
	unsigned char step;
	uint8_t motspeed = 0x00, dir = 0x00, recv = 0xFF;
	step = 0x00;
	LED_OFF();
	portInit();
	rfInit();
	FIFOReset();
	pwm_on_pb1();
	
	//motor beallitasa
	PORTD &= ~(1<<PD5);
	PORTD &= ~(1<<PD7); 
	PORTB &= ~(1<<PB0);
	PORTD &= ~(1<<PD6);
	
	PORTD |= (1<<PD6); 
	delay_ms(10);
	PORTD &= ~(1<<PD6); 
	PORTD |= (1<<PD6); 
	delay_ms(10);
	PORTD &= ~(1<<PD6); 
	

	
	while(1) {

		recv = rfRecv();
		count=0;
		motspeed = recv & 0b00011111;//also 5 bit a sebesseg -> max 31 lehet
		dir = recv >> 5; //felso 3 bit az irany
		
		if(motspeed <= 31) //set motor speed
		{
			if((130/31 * motspeed) < 131)
			{
				OCR1A=130/31 * motspeed;
			}
			
			i++;
			if(i==100){
				LED_ON();
			}
			if(i==200){
				LED_OFF();
				i=0;
			}			
		}
		
		Turn(dir);
		
		if(dir & 0b00000100){ //start motor2
			PORTD |= (1<<PD1);
		}else{ // stopmotor2
			PORTD &= ~(1<<PD1);
		}

		FIFOReset();
	}
	return 0;
}