/*
 * stepper.c
 *
 *  Created on: Mar 20, 2018
 *      Author: ranasl62
 */

#include <avr/io.h>
#include <stdio.h>
#include <util/delay.h>

#define F_CPU 16000000UL /**< Clock speed for delay functions. */

#define FOSC 16000000 /**< Clock speed for UBRR calculation. refer page 179 of 328p datasheet. */
#define BAUD 9600 /**< Baud Rate in bps. refer page 179 of 328p datasheet. */
#define MYUBRR FOSC/16/BAUD-1 /**< UBRR = (F_CPU/(16*Baud))-1 for asynch USART page 179 328p datasheet. Baud rate 9600bps, assuming	16MHz clock UBRR0 becomes 0x0067*/


#define Stepper_PORT PORTC
#define Stepper_DDR DDRC

#define A PC3
#define A_Prime PC2
#define B PC1
#define B_Prime PC0
#define L298_EN PC4

int step_change;
uint8_t direction;
static long int pos;

#define Clockwise 0
#define CounterClockwise 1

int f=0,l=0; //front and end of queue

struct arr{
	char String[3],x;
	int a;
}ax[100];


const uint8_t Full_step_FSM[4]=
{
		/*
			A	A'	B	B'
			1	0	0	0
			1	0	1	0
			0	1	1	0
			0	1	0	1

		  */
		0b1000,
		0b1010,
		0b0110,
		0b0101
};

/**
 * @brief Initialize USART for 8 bit data transmit no parity and 1 stop bit.
 *
 *@details This is a code snippet from datasheet page 182
 *
 * @param ubrr The UBRR value calculated in macro MYUBRR
 * @see MYUBRR
*/
void USART_init(unsigned int ubrr)
{

	UCSR0C = (0<<USBS0)|(3<<UCSZ00); /// Step 1. Set UCSR0C in Asynchronous mode, no parity, 1 stop bit, 8 data bits
	UCSR0A = 0b00000000;/// Step 2. Set UCSR0A in Normal speed, disable multi-proc

	UBRR0H = (unsigned char)(ubrr>>8);/// Step 3. Load ubrr into UBRR0H and UBRR0L
	UBRR0L = (unsigned char)ubrr;


	UCSR0B = 0b00011000;/// Step 4. Enable Tx Rx and disable interrupt in UCSR0B
}

/**
 * @brief Send 8bit data.
 *
 *@details This is a code snippet from datasheet page 184
 *
 * @param data The 8 bit data to be sent
*/

int USART_send(char c, FILE *stream)
{

	while ( !( UCSR0A & (1<<UDRE0)) )/// Step 1.  Wait until UDRE0 flag is high. Busy Waitinig
	{;}

	UDR0 = c; /// Step 2. Write char to UDR0 for transmission
}
/**
 * @brief Receive 8bit sata.
 *
 *@details This is a code snippet from datasheet page 187
 *
 * @return Returns received data from UDR0
*/
int USART_receive(FILE *stream )
{

	while ( !(UCSR0A & (1<<RXC0)) )/// Step 1. Wait for Receive Complete Flag is high. Busy waiting
		;

	return UDR0;/// Step 2. Get and return received data from buffer
}

void init_stepper_ports()
{
	Stepper_DDR |= (1<<A)|(1<<A_Prime)|(1<<B)|(1<<B_Prime)|(1<<L298_EN);
	Stepper_PORT &= ~(1<<L298_EN);// Disable L298
}
uint8_t get_index(long int step_count)
{
	uint8_t idx = 0;
	uint8_t temp_idx=0;
			if(step_count%4 < 0)
			{
				temp_idx = (step_count%4)+4;
			}
			else
				temp_idx = step_count%4;

			if(temp_idx)
				idx = (temp_idx);

	printf("index %d\r",idx);
	return idx;
}
void stepper_FSM(long steps)
{
	static long int step_counter=0;
	long i =0;
	uint8_t bit_mask=0b1111;
	for(i=0;i<steps;i++)
	{
		Stepper_PORT = (Stepper_PORT & ~(bit_mask))|(Full_step_FSM[get_index(step_counter)]);
		if(direction== Clockwise){
			step_counter +=1;
			pos=(pos%200)+1;
		}
		else{
			step_counter-=1;
			pos=(pos%200)-1;
			//printf("%d %d\r",posi,pos);
	}
		_delay_ms(150);
}
}
void pop(){
	if(f<l)f--;
}
void top(){
	if(f<l&&l!=0)
	printf("%s %c %d\r",ax[f].String,ax[f].x,ax[f].a);
}
int main()
{
		USART_init(MYUBRR);
	   // Initialize the standard IO handlers
	    stdout = fdevopen(USART_send, NULL);
	    stdin = fdevopen(NULL, USART_receive);
	    init_stepper_ports();

	Stepper_PORT |= (1<<L298_EN);
	pos=0;
	uint8_t en_dis=1;
	uint8_t q_en=0;
	float unit=0.5;
	long int posi=0;

	while(1)
	{

		int16_t a;
		unsigned char str[3];
		scanf("%s",str);
		printf("%s\r",str);
		if(q_en){
			ax[l].String[0]=str[0];
			ax[l].String[1]=str[1];
			ax[l].String[2]=str[2];
		}
		if(str[0]=='M'&&str[1]=='1'&&str[2]=='7'){
			en_dis=1;
		}
		else if(str[0]=='M'&&str[1]=='1'&&str[2]=='8'){
			en_dis=0;
		}
		else if(en_dis){
			if((str[0]=='M'&&str[1]=='2')||(str[0]=='M'&&str[1]=='3'&&str[2]=='0')){
				return 0;
			}
			else if(str[0]=='M'&&str[1]=='0'&&str[2]=='0')
			{
				q_en=1;
			}
			else if(str[0]=='G'&&str[1]=='2'&&str[2]=='8'){
				//printf("%d %d\r",posi,pos);
				a=((posi-pos)*unit+0.5);
				if(a<0){
							a=a*(-1);
							//printf("Yes CounterClockwise\r");
							//set_direction(CounterClockwise);
							//printf("%d\r",a);
							direction=CounterClockwise;
							stepper_FSM(a);
							_delay_ms(300);
						}
						else {
							//printf("Yes Clockwise\r");
							//set_direction(Clockwise);
							//printf("%d\r",a);
							direction=Clockwise;
							stepper_FSM(a);
							_delay_ms(300);

						}
				//pos=0;

			}
			else if(str[0]=='G' && str[1]=='2' && str[2]=='0'){
				unit=22.86;

			}
			else if(str[0]=='G' && str[1]=='2' && str[2]=='1'){
				unit=0.5;

			}
			else if(str[0]=='G' && str[1]=='9' && str[2]=='0'){
				posi=0;

			}
			else if(str[0]=='G' && str[1]=='9' && str[2]=='1'){
				//printf("%d %d\r",pos,posi);
				posi=pos;
			}
			else if(str[0]=='G' && str[1]=='0' && str[2]=='1'){
				//scanf("%s",&str);
				unsigned char x;
				scanf(" %c",&x);
				scanf("%d",&a);
				if(q_en){
					ax[l].a=a;
					ax[l].x=x;
						}
				a=((a*unit)+0.05);
			//	printf("%d\r",a);
				if(a<0){
					a=a*(-1);
					//printf("Yes CounterClockwise\r");
					//set_direction(CounterClockwise);
					direction=CounterClockwise;
					stepper_FSM(a);
				//	printf("%d %d\r",pos,posi);
					_delay_ms(300);
				}
				else {
					//printf("Yes Clockwise\r");
					//set_direction(Clockwise);
					direction=Clockwise;
					stepper_FSM(a);
					_delay_ms(300);

				}
				//printf("%d %d\r",pos,posi);
			}
			Stepper_PORT=0x00;//release power to motor
			if(q_en){
				l++;
			}
		}



	}
}