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

#define bit(x) (1 << x)
// Tabela de Números
const unsigned char Tabela[] PROGMEM = {
  0b1000000, // 0
  0b1111001, // 1
  0b0100100, // 2
  0b0110000, // 3
  0b0011001, // 4
  0b0010010, // 5
  0b0000010, // 6
  0b1111000, // 7
  0b0000000, // 8
  0b0011000, // 9
};

// Valores display
unsigned char display1 = 0; // 0 - 1
unsigned char display2 = 0; // 0 - 8
unsigned char display3 = 0; // 0 - 9 
unsigned char display_value = 0;

// Variavel servo motor
unsigned char grau_motor = 0;

// ============== FUNÇÕES DISPLAY ============= //
void showDisplay() {
  display3 = display_value%10;
  if(display_value > 99) {
    display1 = 1;
    display2 = (display_value/10)%10;
  } else {
    display1 = 0;
    display2 = display_value/10;
  }
  
  // Ativa as portas do display1
  PORTD = pgm_read_byte(&Tabela[display1]);
  PORTB |= bit(PB5);
  _delay_ms(5);
  PORTB &= ~(bit(PB5));

  // Ativa as portas do display2
  PORTD = pgm_read_byte(&Tabela[display2]);
  PORTB |= bit(PB4);
  _delay_ms(5);
  PORTB &= ~(bit(PB4));

  // Ativa as portas do display3
  PORTD |= pgm_read_byte(&Tabela[display3]);
  PORTB |= bit(PB3);
  _delay_ms(5);
  PORTB &= ~(bit(PB3));

}

// ============== SERVO MOTOR ============= //
void moveMotorDir(unsigned char grau) {
  if (grau == 180)
    return;

  if (!grau) {
    OCR1A = 1092;
    return;
  }

  if (grau <= 9 && grau < 45)
    OCR1A += 191;
  else if (grau <= 90 &&  grau < 135)
    OCR1A += 208;
  else 
    OCR1A += 205;

}

void moveMotorEsq(unsigned char grau) {
  if (grau <= 0)
    return;

  if (grau <= 1 && grau < 6) 
    OCR1A -= 191;
  else if (grau <= 11 && grau < 15)
    OCR1A -= 208;
  else {
    OCR1A -=205;
  }
}


// ================= MAIN ================== //
int main() {
  // Displays
  DDRB |= bit(PB3) | bit(PB4) | bit(PB5);
  PORTB &= ~(bit(PB5) | bit(PB4) | bit(PB3));
  DDRD |= 0xFF;
  PORTD = 0xFF;

  // PWM
  // -- Ativação do pino 
  DDRB |= bit(PB1); 
  PORTB &= ~(bit(PB1));

  // Seguindo o protocolo padrão usado pela maioria dos Servo motores
  // vamos iniciá-los com 20ms e 1ms 
  ICR1   = 39999; // 20 ms
  OCR1A  = 1999; // 1 ms

  // Ativa PWM no modo rápido com preescaler de 8
  // WGM11, WGM12 e WGM13 -> PWM modo rápido
  // CS11                 -> Preescaler 8
  TCCR1A = bit(WGM11);
  TCCR1B = bit(CS11) | bit(WGM12) | bit(WGM13);  

  // A saída do pino deve ser configurada
  // somente após configurar o modo de operação do pino
  TCCR1A |= (1 << COM1A1);
  sei(); // Ativa interrupções

  // Coloca o servo motor em 0 graus
  OCR1A = 1092;

  while(1) {
    // showDisplay();
    grau_motor += 9;
    if (grau_motor > 180)
      grau_motor = 0;
    moveMotorDir(grau_motor);
    _delay_ms(1000);
  }
}