#include <stdint.h>           // standard integer types

#include <stdio.h>

#include <stdlib.h>

#include <avr/io.h>

#include <avr/interrupt.h>

#include <util/delay.h>

#include "uart2.h"

int servo, drat, vystup, hodnota, rychlost_imp, rychlost,xtalPresc, poloha;

// ============================= DEFINE ===========================

#define SETBIT(ADDRESS,BIT) ((ADDRESS) |= (1<<(BIT)))	// nastaví bit
#define CLRBIT(ADDRESS,BIT) ((ADDRESS) &= ~(1<<(BIT)))	// nuluje bit
//#define NEGBIT(ADDRESS,BIT) ((ADDRESS) ^= (1<<(BIT)))	// neguje bit
//#define TSTBIT(ADDRESS,BIT) ((ADDRESS) & (1<<(BIT)))	// testuje bit

#define PWM1_PORT PORTA
#define PWM1_DDR  DDRA
#define PWM1_BIT  PA7	// PA7 = ADC7

/* Pointer implementation of the FIFO */
int *PUTPT;    /* Pointer of where to put next */
int Fifo[20]; /* The statically allocated fifo data */

// =========================== INIT PORTY =========================

void init_port(void)
{
  DDRA  = 0B10000000;	// nastaveni portu PA7 jako vystup, jinak vstup
  PORTA = 255;	

  DDRD  = 0B00100011;	// nastaveni vystupy pro seriovou linku a ICR1A (PWM2)
  PORTD = 255;	
}

// =========================== ADC READ ===========================

int read_ADC1(void)
{
      ADMUX = 0b00000000;      //kanál 0
      ADCSRA = 0b10000101;

      ADCSRA |= (1<<ADSC); //Start conversion
      while ((ADCSRA & (1 << ADSC)) == 1){};
      return ADCW;
}
int read_ADC2(void)
{
      ADMUX = 0b00000001;      //kanál 1
      ADCSRA = 0b10000101;

      ADCSRA |= (1<<ADSC); //Start conversion
      while ((ADCSRA & (1 << ADSC)) == 1){}; 
      servo = 173+ADCW*172/1024; //cteni z prevodniku a uprava hodnoty 1024 na rozsah 173-345 pro PWM2
      return servo;
}
// ========================= INIT FIFO ============================
void init_fifo(void) 
{
    PUTPT=&Fifo[0];
    
}
void PutFifo (int data) 
{   
    int *Ppt; /* Temporary put pointer */
    Ppt=PUTPT; /* Copy of put pointer */
    *(Ppt++)=data; /* Try to put data into fifo */
    if (Ppt == &Fifo[20]) Ppt = &Fifo[0]; /* Wrap */
	PUTPT=Ppt;
        
}

// ========================= INIT PWM =============================

void init_PWM1(void)
{
      TCCR0 = 0b01001011;
	  TIMSK = 0b00000011;

      OCR0 = 0;
	  TCNT0 = 0;

}

void init_PWM2(void)
{
      
      TCCR1A = 0b10000010;          // 50 Hz -> 20ms / 3455 dílků
      TCCR1B = 0b00011011;
      
      ICR1 = 3455;

      TCNT1  = 0;
      OCR1A = 0; 
}
// =========================== PWM ================================

void PWM1(int pulse1)
{
      
      OCR0 = pulse1; 

}


void PWM2(int pulse2)
{
      
      OCR1A = pulse2; 

}

// ========== Preruseni PWM1 pro zmenu portu na AD7 ===============

ISR(TIMER0_OVF_vect)
{
	// Timer/Counter0 Overflow

	if(OCR0) SETBIT(PWM1_PORT, PWM1_BIT);
		// pokud je generovaná hodnota nenulová, nastaví se výstup na "1"
}

ISR (TIMER0_COMP_vect)
{
	// Timer/Counter0 Output Compare Match

	CLRBIT(PWM1_PORT, PWM1_BIT);
}
// ========================== PROG ================================

int main(void)
{
      int prum;

      uart_init();

      init_port();
	  
      init_PWM1();
      init_PWM2();
      init_fifo();

for(int i=0;i<15;i++) {        // nacteni 15 hodnot do buferu z ADC1
	PutFifo(read_ADC1());
	_delay_ms(50);
}
	  
	while(1)
	{
		PutFifo(read_ADC1());   //nacteni sestnacte hodnoty a vypocteny prumer (FIR filtr)
			
		for(int i=0;i<16;i++) prum=+Fifo[i];
		prum=prum/16*256/1024;
		
		PWM1(prum);
  
        PWM2(read_ADC2());               // zadani hodnoty PWM2 (-90°/0°/90° -> 173 - 259 - 345)
    
		_delay_ms(250);  
		                      
	  sei(); //povoleno preruseni

	}
}