#include <mega16.h>
#include <delay.h> 

#include <stdio.h>  // Standard Input/Output functions
#include <math.h>   // matematicke funkce

unsigned char Enc1a,Enc1b,Enc1c,Enc1d,Enc2a,Enc2b,Enc2c,Enc2d,baterie,bat,M1I,M2I,stav,UhelCHR,BezpSoucet;  
int PocitadloZnaku,UhelDEG,uh;
char Znak,rychlost,Smer, Cidla, Zabezpeceni,Nasobek;
long int ENC1,ENC2;

unsigned char PrijateSlovo[10];
unsigned char BAT[5];

int POM;

// I2C Bus functions
#asm
   .equ __i2c_port=0x1B ;PORTA
   .equ __sda_bit=6
   .equ __scl_bit=7
#endasm
#include <i2c.h>

// Alphanumeric LCD Module functions
#asm
   .equ __lcd_port=0x15 ;PORTC
#endasm
#include <lcd.h>

#define RXB8 1
#define TXB8 0
#define UPE 2
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7

#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<OVR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)

// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];


void IICzapis(unsigned char adresa, unsigned char data) //I2C pro desku MD23
{
  i2c_start();
  i2c_write(0xB0);
  delay_ms(2);
  i2c_write(adresa); 
  delay_ms(2);
  i2c_write(data);delay_ms(5);
  i2c_stop();
  delay_ms(3);
}              

unsigned char IICcteni(unsigned char adresa) //I2C pro desku MD23
{ 
unsigned char Data;
  i2c_start();
  i2c_write(0xB0); 
delay_ms(5);
  i2c_write(adresa);
delay_ms(5);
  i2c_start();
  i2c_write(0xB1); 
delay_ms(5);
  Data=i2c_read(0);
  i2c_stop();
  return Data;                    
  
}

void IICzapis2(unsigned char adresa, unsigned char data) //I2C pro desku CMPS03
{
  i2c_start();
  i2c_write(0xC0);
  delay_ms(2);
  i2c_write(adresa); 
  delay_ms(2);
  i2c_write(data);delay_ms(5);
  i2c_stop();
  delay_ms(3);
}              

unsigned char IICcteni2(unsigned char adresa) //I2C pro desku CMPS03
{ 
unsigned char Data;
  i2c_start();
  i2c_write(0xC0); 
delay_ms(5);
  i2c_write(adresa);
delay_ms(5);
  i2c_start();
  i2c_write(0xC1); 
delay_ms(5);
  Data=i2c_read(0);
  i2c_stop();
  return Data;                    
  
}

 void ZjistiHodnoty(void)
 {
 
 baterie=IICcteni(10); 
 UhelCHR=IICcteni2(1);                               
        
        Enc1a=IICcteni(2);
        Enc1b=IICcteni(3);
        Enc1c=IICcteni(4);
        Enc1d=IICcteni(5);  
        
        Enc2a=IICcteni(6); 
        Enc2b=IICcteni(7);
        Enc2c=IICcteni(8);
        Enc2d=IICcteni(9);  
     
        ENC1=Enc1c;
        ENC1<<=8;
        ENC1=ENC1+Enc1d;    
        
        ENC2=Enc2c;
        ENC2<<=8;
        ENC2=ENC2+Enc2d; 
          
        M1I=IICcteni(11);
        M2I=IICcteni(12);
        BezpSoucet=baterie+ENC1+ENC2+ M1I+M2I+PINB.0+PINB.1+PINB.2+PINB.3+PINB.4+stav+UhelCHR;
       
          printf("#%03d%06d%06d%03d%d%d%d%d%d%d%03d%03d*\n%c ",baterie,ENC1,ENC2, M1I+M2I,PINB.0,PINB.1,PINB.2,PINB.3,PINB.4,stav,UhelCHR,BezpSoucet,13);    //ENC1 a ENC2 nabyvaji hodnot int coz je 5 mist a znamenko je 6te misto  
        
         // printf("B:%03d E1:%03d%03d%03d%03d E2:%03d%03d%03d%03d I1:%03d I2:%03d \n %c ",baterie,Enc1a,Enc1b,Enc1c,Enc1d, Enc2a,Enc2b,Enc2c,Enc2d, M1I,M2I,13);      
    
         lcd_gotoxy(13,0);
         lcd_putsf(". V");
         bat=ceil(baterie/100);  
         baterie=baterie-bat*100;
         lcd_gotoxy(11,0); 
         lcd_putchar(bat+48);
         bat=ceil(baterie/10);  
         baterie=baterie-bat*10;
         lcd_gotoxy(12,0); 
         lcd_putchar(bat+48);  
         bat=ceil(baterie);  
         baterie=baterie-bat;
         lcd_gotoxy(14,0); 
         lcd_putchar(bat+48);
         
         UhelCHR=IICcteni2(1); 
          
         UhelDEG=UhelCHR*1411; 
         
         uh=ceil(UhelCHR/100); 
         lcd_gotoxy(1,1); 
         lcd_putchar(uh+48);
         UhelCHR=UhelCHR-uh*100;
         
         uh=ceil(UhelCHR/10); 
         lcd_gotoxy(2,1); 
         lcd_putchar(uh+48);
         UhelCHR=UhelCHR-uh*10; 
         
         uh=ceil(UhelCHR); 
         lcd_gotoxy(3,1); 
         lcd_putchar(uh+48);
      
 }


#if RX_BUFFER_SIZE<256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif

// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;

// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
   {
   rx_buffer[rx_wr_index]=data;
   if (++rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
   if (++rx_counter == RX_BUFFER_SIZE)
      {
      rx_counter=0;
      rx_buffer_overflow=1;
      };
   };
   
   
   if((data=='#') || (data=='@') || (data=='$'))
   {
     PocitadloZnaku=0;
   }                  
   
   PocitadloZnaku++;
   PrijateSlovo[PocitadloZnaku]=data;
 
}

#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index];
if (++rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif

// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];

#if TX_BUFFER_SIZE<256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif

// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
   {
   --tx_counter;
   UDR=tx_buffer[tx_rd_index];
   if (++tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
   {
   tx_buffer[tx_wr_index]=c;
   if (++tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
   ++tx_counter;
   }
else
   UDR=c;
#asm("sei")
}
#pragma used-
#endif


void Stop(void)
{
  IICzapis(0,128);
  IICzapis(1,128);
}

void Vpred(unsigned char R)
{
  IICzapis(0,128+R); 

} 
void Vzad(unsigned char R)
{
  IICzapis(0,128-R); 

}
void Vlevo(unsigned char R)
{
  IICzapis(1,128-R/2); 

}
void Vpravo(unsigned char R)
{ 
  IICzapis(1,128+R/2);

}

void JedDrahu(void)
{ 
  unsigned int rychlostPamatuj, DrahovyRozdil;
  long int Draha;
  char Hotovo;
  rychlostPamatuj=rychlost;
  rychlost=30;
  Hotovo=0; 
  Smer=PrijateSlovo[2];  
  
  //Draha=(PrijateSlovo[3]-48)*10000+(PrijateSlovo[4]-48)*1000+(PrijateSlovo[5]-48)*100+(PrijateSlovo[6]-48)*10+(PrijateSlovo[7]-48);
  
  Draha= (PrijateSlovo[3]-48);
  Draha=Draha*10 + (PrijateSlovo[4]-48);
  Draha=Draha*10 + (PrijateSlovo[5]-48);
  Draha=Draha*10 + (PrijateSlovo[6]-48);
  Draha=Draha*10 + (PrijateSlovo[7]-48);
       
  for(;Hotovo==0;)
  {  
     switch (Smer)
          {
          case 0: Stop();
                  break;
          case 1: Vpred(rychlost);
                  break;  
          case 2: Vzad(rychlost);
                  break;          
          case 3: Vlevo(rychlost);
                  break;    
          case 4: Vpravo(rychlost);
                  break;
          } 
          
          DrahovyRozdil=Draha-((abs(ENC1)+abs(ENC2))/ 2);
          
          if ( (DrahovyRozdil<=250) &&(DrahovyRozdil>100))
          {
           rychlost=12;
          } 
          if ( (DrahovyRozdil<=100) )
          {
           rychlost=7;
          }
          
           Cidla= PINB.0+ PINB.1*2+ PINB.2*4+ PINB.3*8+PINB.4*16;
           if (Cidla != 31) //nekde je prekazka
           {
             stav = 2;
             Stop(); 
             Smer=0;
             PrijateSlovo[1]='A';
             PocitadloZnaku=0;
             break;
           } 
           
           
           ZjistiHodnoty();    
                
           if( ((abs(ENC1)+abs(ENC2))/ 2)  >=  Draha)
           {
             
             Stop();
             Smer=0;
             Hotovo=1;
             PocitadloZnaku=0;
             delay_ms(500);
             IICzapis(16,32); // snuluj pocitadla
             rychlost=rychlostPamatuj; // nastav puivodni rychlost
             //break;
           } 
   }
 stav=0;
 PrijateSlovo[1]='A';
 PocitadloZnaku=0;
}


// Timer 0 overflow interrupt service routine
interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{
// Place your code here   


}

// Timer 1 overflow interrupt service routine
interrupt [TIM1_OVF] void timer1_ovf_isr(void)
{
// Place your code here 
POM++; 
  



}

// Timer 2 overflow interrupt service routine
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{
// Place your code here

}

// Declare your global variables here

void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T 
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T 
PORTB=0xFF;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T 
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T 
PORTD=0x00;
DDRD=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 10,800 kHz
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x05;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 10,800 kHz
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: On
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x03;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 10,800 kHz
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x07;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x45;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud rate: 9600
UCSRA=0x00;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x47;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// I2C Bus initialization
i2c_init();

// LCD module initialization
lcd_init(16);
lcd_clear();  

lcd_gotoxy(0,0);
lcd_putchar('A'); 
lcd_putchar('B');

	

	

IICzapis(14,10);  //  nabehy motoru  1-pomalu 5-default 10-nejrychleji
delay_ms(20);
IICzapis(16,0x20);      // reset pocitadel enkoderu     
delay_ms(20);
IICzapis(15,2); 
delay_ms(20);
IICzapis(1,128); 
delay_ms(20);
IICzapis(0,128);
delay_ms(10); 
rychlost=20;
stav=0;

// Global enable interrupts
#asm("sei")


POM=0;
while (1)   // nekonecna smycka
      {
     Cidla= PINB.0+ PINB.1*2+ PINB.2*4+ PINB.3*8+PINB.4*16;
     if (Cidla != 31) //nekde je prekazka
     {
      stav = 2;
      Stop();
      Smer=0;
      PrijateSlovo[1]='A'; //znehodnot prikaz z PC
      PocitadloZnaku=0;
     }
  
     ZjistiHodnoty();
     
     if(PocitadloZnaku==9 && (PrijateSlovo[1]=='$') && (PrijateSlovo[9]=='*'))
     {
      Zabezpeceni=PrijateSlovo[2]+(PrijateSlovo[3]-48)+(PrijateSlovo[4]-48)+(PrijateSlovo[5]-48)+(PrijateSlovo[6]-48)+(PrijateSlovo[7]-48);
      if (Zabezpeceni >=10)
      {
        Nasobek=(Zabezpeceni/10);
        Zabezpeceni=Zabezpeceni-Nasobek*10;
      }
      
      if(Zabezpeceni == (PrijateSlovo[8]-48))
      {
        stav=1;
        ZjistiHodnoty();
        delay_ms(500);
        JedDrahu();  
      }
     }
     
      if(PocitadloZnaku==4);
      { 
        if((PrijateSlovo[1]=='#') && (PrijateSlovo[4]=='*') && (PrijateSlovo[2]==PrijateSlovo[3]) )
        {
          Smer=PrijateSlovo[2];
        }
        
        if((PrijateSlovo[1]=='@') && (PrijateSlovo[4]=='*') && (PrijateSlovo[2]==PrijateSlovo[3]) )
        {
          switch(PrijateSlovo[2])
            {
              case 5:rychlost=5;                
                  break;  
              case 6:rychlost=15;
                  break; 
              case 7:rychlost=30;
                  break; 
              case 8:rychlost=50;
                  break; 
              case 9:rychlost=80;
                  break;    
            }
        }
        
        switch (Smer)
          {
          case 0: Stop();
                  break;
          case 1: Vpred(rychlost);
                  break;  
          case 2: Vzad(rychlost);
                  break;          
          case 3: Vlevo(rychlost);
                  break;    
          case 4: Vpravo(rychlost);
                  break;
          case 'X': IICzapis(16,32);
                   break;    
          }
      
      } 
      
      // Place your code here

      };
}
