#ifndef F_CPU
#define F_CPU 16000000UL
#endif
#define __DELAY_BACKWARD_COMPATIBLE__

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

#define LCD_PRT PORTB
#define LCD_DDR PORTB 
#define LCD_PIN PORTB 
#define LCD_RS 0
#define LCD_RW 1
#define LCD_EN 2

void delay_us(int d);
void delay_ms(int d);
void lcd_init();
void lcdCommand( unsigned char cmnd );
void lcdData( unsigned char data );
void lcd_gotoxy(unsigned char x, unsigned char y);
void lcd_print( const char * str );
void print(const char *string);
void usart_init (void);
void usart_send (unsigned char ch); 
unsigned char usart_receive(void); 
unsigned int get_number(int number[], int digits);
void move_stepper(int status);
char key_teclapad();
const char tecla[4][3] = {
{'1','2','3'}, 
{'4','5','6'}, 
{'7','8','9'}, 
{'*','0','#'}}; 
int pres = 0; //Variable parasaber si el boton se presiona 
char STEPS[4] = {0b1000, 0b0100, 0b0010, 0b0001}; // ABCD Stepper
int step = 0;  
int STATUS=0, N_STEPS=0; 
int pantalla = 0; // Inicializa la pantalla en 0 para ver si hay cambios

void delay_us(int d)
{
  _delay_us(d);
}
void delay_ms(int d)
{
  _delay_ms(d);
}

void lcd_init(){
  LCD_DDR = 0xFF; //LCD port is output
  LCD_PRT &=~(1<<LCD_EN); //LCD_EN = 0
  delay_us(2000); //wait for stable power
  lcdCommand(0x33); //$33 for 4-bit mode
  delay_us(200); //wait
  lcdCommand(0x32); //$32 for 4-bit mode
  delay_us(200); //wait
  lcdCommand(0x28); //$28 for 4-bit mode
  delay_us(200); //wait
  lcdCommand(0x0e); //display on, cursor on
  delay_us(200); //wait
  lcdCommand(0x01); //clear LCD
  delay_us(2000); //wait
  lcdCommand(0x06); //shift cursor right
  delay_us(200);
}

void lcdCommand( unsigned char cmnd ){
	LCD_PRT = (LCD_PRT & 0x0F) | (cmnd & 0xF0);
  LCD_PRT &= ~ (1<<LCD_RS); //RS = 0 for command
  LCD_PRT &= ~ (1<<LCD_RW); //RW = 0 for write
  delay_us(100);
  LCD_PRT |= (1<<LCD_EN); //EN = 1 for H-to-L
  delay_us(100); //wait to make EN wider
  LCD_PRT &= ~ (1<<LCD_EN); //EN = 0 for H-to-L
  LCD_PRT = (LCD_PRT & 0x0F) | (cmnd << 4);
  delay_us(100); //wait
  LCD_PRT |= (1<<LCD_EN); //EN = 1 for H-to-L
  delay_us(100); //wait to make EN wider
  LCD_PRT &= ~ (1<<LCD_EN); //EN = 0 for H-to-L
}

void lcdData( unsigned char data ){
  LCD_PRT = (LCD_PRT & 0x0F) | (data & 0xF0);
  LCD_PRT |= (1<<LCD_RS); //RS = 1 for data
  LCD_PRT &= ~ (1<<LCD_RW); //RW = 0 for write
  delay_us(100); //wait
  LCD_PRT |= (1<<LCD_EN); //EN = 1 for H-to-L
  delay_us(100); //wait
  LCD_PRT &= ~ (1<<LCD_EN); //EN = 0 for H-to-L
  LCD_PRT = (LCD_PRT & 0x0F) | (data << 4);
  delay_us(100); //wait
  LCD_PRT |= (1<<LCD_EN); //EN = 1 for H-to-L
  delay_us(100); //wait
  LCD_PRT &= ~ (1<<LCD_EN); //EN = 0 for H-to-L
}

void lcd_gotoxy(unsigned char x, unsigned char y){ 
  unsigned char firstCharAdr[] = {0x80, 0xC0, 0x94, 0xD4}; 
  lcdCommand(firstCharAdr[y-1] + x - 1);
  delay_us(100);
}

void lcd_print( const char * str ){
  unsigned char i = 0;
  while(str[i]!=0)
  {
    if (str[i]=='\n'){
      lcd_gotoxy(1,2);
      i++;
    }
    else{
      lcdData(str[i]);
      i++;  
    }
  }
}

void print(const char * string){
  lcd_print(string);
  while (*string!='\0'){
    usart_send(*string++);
  }
}

void usart_init (void){
  UCSR0B = (1<<TXEN0)|(1<<RXEN0);
  UBRR0L = 0x67; //9600 baud rate
  UCSR0C = 0x06;
}

void usart_send (unsigned char ch){
  while (! (UCSR0A & (1<<UDRE0)));
  UDR0 = ch; //transmite
}

unsigned char usart_receive(void){
  
  if ((UCSR0A & (1<<RXC0))!=0){
    return UDR0;
  }
  return '\0';
}

unsigned int get_number(int number[], int digits){
	unsigned int result = 0;
	int i=0;

	for (i=0; i<digits; i++){
		result += (number[i])*pow(10, digits-1-i);
	}
	return result;
}

void move_stepper(int status){
	PORTD = STEPS[step];
	if (status==0){
		step--;
	}
	else{	
		step++;
	}
		
	if (step==-1){
		step = 3;
	}
	else if (step==4){
		step = 0;
	}
}

char key_teclapad(){
	int column;
	for (column=0; column<3; column++){
		PORTK = (~(1 << (column+4)))|(0b00001111); // Pull-ups
		
		if (!(PINK&0x01)){
			delay_ms(100);
			return tecla[0][column];
		}
		else if (!(PINK&0x02)){
			delay_ms(100);
			return tecla[1][column];
		}
		else if (!(PINK&0x04)){
			delay_ms(100);
			return tecla[2][column];
		}
		else if (!(PINK&0x08)){
			delay_ms(100);
			return tecla[3][column];
		}
	}
	delay_ms(100);
	pres = 0;
	return ' ';
}

int main(void) {
	DDRB = 0xFF;// Pull ups para el teclado
	PORTK = 0b00001111; // Pull ups para [A,B,C,D]
	DDRK = 0b01110000; 
	
 //-Para el PWM (Servo)
	DDRH |= ( 1<< PH3 ); 
	TCNT4 = 0;
	ICR4 = 39999; 
	TCCR4A =  (1 << COM1A1) | (0 << COM1A0) ; 
	TCCR4A |=  (1 << WGM11) | (0 << WGM10) ; 
	TCCR4B = (1 << WGM13) | (1 << WGM12); 
	TCCR4B |= (0 << CS12) | (1 << CS11) | ( 0 << CS10 ); 
	
	float degree = 0;
	OCR4A = (float)100/9*(degree)+1999;
	DDRD = 0b00001111;

 //-Interrupciones
	TCCR0B = 0x04; //Preescalador de 256
	TIMSK0 = (1<<TOIE0); 
	sei (); 	// Interrupciones
	usart_init(); // Iniciar el USART
	lcd_init(); // Iniciar la LCD
  lcdCommand(0x01); //clear LCD
	delay_us(2000);

	char string[50] = "";
	unsigned char input, tecla;
	int sig=0; 
	strcpy(string, "1. Control\n2. Status");
	print(string);

	while(1){
		input = usart_receive();
		tecla = key_teclapad();
		
		if (input!='\0' || (tecla!=' ' && pres==0)){
			sig = 1;
			pres = 1;
		}
		if (sig==1){
			if (pantalla==0){		
				if (tecla=='1' || input=='1'){
					pantalla = 2;
				}
				else if (tecla=='2' || input=='2'){
					pantalla = 1;
				}
				else if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
			}
			else if (pantalla==1){
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
			}
			else if (pantalla==2){
				strcpy(string, "1. Stepper\n2. Servomotor");
				usart_send('\n');
				usart_send('\n');
				print(string);
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
				else if (tecla=='1' || input=='1'){
					pantalla = 5;
				}
				else if (tecla=='2' || input=='2'){
					pantalla = 3;
				}
			}
			else if (pantalla==3){
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
				else if (tecla=='1' || input=='1'){
					pantalla = 4; // Rotate
				}
				else if (tecla=='2' || input=='2'){
					pantalla = 0; // Stop
				}
			}
			else if (pantalla==4){
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
			}
			else if (pantalla==5){
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
				else if (tecla=='1' || input=='1'){
					pantalla = 6; // Direccion de pasos
				}
				else if (tecla=='2' || input=='2'){
					pantalla = 0;
					// Stop stepper
					N_STEPS = 0;
					TCNT0 = 0x00;
				}
			}
			else if (pantalla==6){
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
				else if (tecla=='1' || input=='1'){
					pantalla = 7;
					STATUS = 0;
					TCNT0 = 0;
				}
				else if (tecla=='2' || input=='2'){
					pantalla = 7;
					STATUS = 1;
					TCNT0 = 0;
				}
			}
			else if (pantalla==7){
				if (tecla=='*' || input=='*'){
					pantalla = 0;
				}
			}

			lcdCommand(0x01); //clear LCD
			delay_us(2000); //wait

			
			if (pantalla==0){ //Menu
				strcpy(string, "1. Control\n2. Status");
				usart_send('\n');
				usart_send('\n');
				print(string);
			}
		
			else if (pantalla==1){ //Status de los motores
				if (N_STEPS!=0){
					sprintf(string, "Stepper: ON\nServo: %d", (int)degree);
				}
				else{
					sprintf(string, "Stepper: OFF\nServo: %d", (int)degree);	
				}
				usart_send('\n');
				usart_send('\n');
				print(string);
			}
		
			else if (pantalla==2){ // Mover Servo o Stepper
				strcpy(string, "1. Stepper\n2. Servomotor");
				usart_send('\n');
				usart_send('\n');
				print(string);
			}
			
			else if (pantalla==3){ //Opcion Servo
				strcpy(string, "1. Rotate\n2. Stop");
				usart_send('\n');
				usart_send('\n');
				print(string);
			}
		
			else if (pantalla==4){ // Angulo Servo 0-180
				strcpy(string, "Angle: ");
				usart_send('\n');
				print(string);
				pres = 1;
				// Input del numero
				int entry[10]={0}, position=0;
				while (1){
					input = usart_receive();
					tecla = key_teclapad();
					if (input=='*' || tecla=='*'){
						pantalla = 0;
						break;
					}
					else if (input=='#'||tecla=='#'){
						degree = get_number(entry, position);
						OCR4A = (float)100/9*degree+1999; // Formula del OCR para los grados
						pantalla = 0;
						lcdCommand(0x01); //clear LCD
						delay_us(2000); //wait
						strcpy(string, "1. Control\n2. Status");
						usart_send('\n');
						usart_send('\n');
						print(string);
						break;
					}
					else if (input!='\0'){
						int number = input-'0';
						if (number>=0 && number<=9){ 
							entry[position] = input-'0';
							position += 1;
							sprintf(string, "%c", input);
							print(string);
						}
					}
					else if (tecla!=' ' && pres==0){ 
						int number = tecla-'0';
						if (number>=0 && number<=9){
							entry[position] = tecla-'0';
							position += 1;
							sprintf(string, "%c", tecla);
							print(string);
						}
						pres = 1;
					}
				}
			}

			else if (pantalla==5){ //Opcion Stepper
				strcpy(string, "1. Rotate\n2. Stop");
				usart_send('\n');
				usart_send('\n');
				print(string);
			}
			
			else if (pantalla==6){ //Sentido de giro
				strcpy(string, "1. Rotate Left\n2. Rotate Right");
				usart_send('\n');
				usart_send('\n');
				print(string);
			}
	
			else if (pantalla==7){ //Numeros de pasos
				strcpy(string, "# of Steps: ");
				usart_send('\n');
				print(string);
				pres = 1;

				// Entrada de numeros 
				int entry[10]={0}, position=0;
				while (1){
					input = usart_receive();
					tecla = key_teclapad();
					if (input=='*' || tecla=='*'){
						pantalla = 0;
						break;
					}
					else if (input=='#'||tecla=='#'){
						N_STEPS = get_number(entry, position);
						TCNT0 = 131;
						TCCR0B = 0x04; //Normal mode, int clk, prescaler 256
						TIMSK0 = (1<<TOIE0); //enable Timers 0 int.
						sei (); // Start interruptions
						pantalla = 0;
						lcdCommand(0x01); //clear LCD
						delay_us(2000); //wait
						strcpy(string, "1. Control\n2. Status");
						usart_send('\n');
						usart_send('\n');
						print(string);
						break;
					}
					else if (input!='\0'){
						int number = input-'0';
						if (number>=0 && number<=9){
							entry[position] = input-'0';
							position += 1;
							sprintf(string, "%c", input);
							print(string);
						}
					}
					else if (tecla!=' ' && pres==0){
						int number = tecla-'0';
						if (number>=0 && number<=9){
							entry[position] = tecla-'0';
							position += 1;
							sprintf(string, "%c", tecla);
							print(string);
						}
						pres = 1;
					}
				}
			}
			sig = 0;
		}
	}
	return 0;
}

// Interrupcion para Stepper
ISR (TIMER0_OVF_vect){	
	if (N_STEPS>0){
		move_stepper(STATUS);
		TCNT0 = 131;
		N_STEPS--;
	}
	else{
		TCNT0 = 0x00;
	}
}