#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 <math.h>	//gaus

#include "uart2.h"

// ============================= 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

#define AVG_BUF_EXP 4
#define AVG_BUF_LEN 16 // AVG_BUF_LEN = 2^AVG_BUF_EXP = 16 for AVG_BUF_EXP == 4 
#define FIR_BUF_LEN 11

int servo;

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

void init_port(void)
{
	DDRA  = 0B10000000;	
	PORTA = 255;	

	DDRD  = 0B00100011;	
	PORTD = 255;	
}

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

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

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

	ADCSRA |= (1<<ADSC); //Start conversion
	while ((ADCSRA & (1 << ADSC)) == 64){}; 
	servo = 173+ADCW*43/256; //cteni z prevodniku a uprava hodnoty 1024 na rozsah 173-345 pro PWM2
	return servo;
}
// ========================= INIT AVG ============================

int avg(unsigned int newVal)
{
   // funkce počítající klouzavý průměr z AVG_BUF_LEN-ti prvků
   // musí platit: max(newVal)*AVG_BUF_LEN < 65535 !!!
   // tedy např. pro 10bit vstup (max = 1023) smí být buffer dlouhý až 64 prvků (1023*64 < 65535)
   // Kdyby nestačil limit 65535, je nutné změnit "sum" na "long int" a modifikovat dle toho kód.

	static unsigned int buff[AVG_BUF_LEN];
	static unsigned int sum = 0;
	static unsigned int *pLast = &(buff[0]); 
	
	sum -= *pLast;
	sum += newVal;
	*pLast = newVal;
	pLast++;
	if(pLast > &(buff[AVG_BUF_LEN-1])) {pLast = &(buff[0]);}   // end of buffer
	return ((sum/AVG_BUF_LEN));  //( AVG_BUF_LEN / 2)) >> (AVG_BUF_EXP));   // divide by AVG_BUF_LEN
}
// ======================== INIT GAUSS ===========================

unsigned int gauss(unsigned int newSample)
{
   // 11th order FIR filter
   // y[n] = b0*x[n] + b1*x[n-1] + ...
   //                        b10 b9 b8  b7 ....                 b1 b0
   static float coef[] = {0.0036,    0.0160,    0.0514,    0.1183,    0.1950,    0.2303,    0.1950,    0.1183,    0.0514,    0.0160,    0.0036};   // N(0,3)
                        // filter coefitients (gaussian), requires sum(coef) = 65535/255 = 257
                        // same length as "buff"
   static unsigned int buff[FIR_BUF_LEN];
   float sum = 0;
   unsigned int i;

   for(i=0; i<FIR_BUF_LEN-1; i++)   // posunout zpozdovaci linku
   {
      buff[i] = buff[i+1];
   }
   buff[FIR_BUF_LEN - 1] = newSample;   // vlozit novy vzorek
   
   for(i=0; i<FIR_BUF_LEN; i++)   // spocitat sumu
   {
      sum+= (float)buff[i] * coef[i];
   }

   // divide by 256
   //return sum/4;
   //if(sum < 65408) 
   return (unsigned int) sum/4;
   //else return (unsigned char)(sum >> 8);
}

// ========================= 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;
                                    // -90°/0°/90° -> 173 - 259 - 345 (250 je stred)
		                            //                
	ICR1 = 3455;

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

void PWM1(int pulse1)
{
      
	OCR0 = pulse1; 

}


void PWM2(int pulse2)
{
      
	OCR1A = pulse2; 

}


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) //ggg
{

    uart_init();
    init_port();
    init_PWM1();
    init_PWM2();
	

	while(1){
		
		//PWM1(read_ADC1()/4);
		//PWM1(avg(read_ADC1())/4);             
		PWM1(gauss(read_ADC1()));
		PWM2(read_ADC2());                      
		                      
		sei(); //povoleno preruseni
	}



}