#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 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, prumer;

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

void init_port(void)
{
	DDRA  = 0B10000000;	
	PORTA = 255;	
	
	DDRB  = 0B00001000;	
	PORTB = 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 / 2)) >> (AVG_BUF_EXP));  //( AVG_BUF_LEN / 2)) >> (AVG_BUF_EXP));   // divide by AVG_BUF_LEN
}
// ======================== INIT GAUSS ===========================

unsigned char gauss(unsigned int newSample)
{
   // 11th order FIR filter
   // y[n] = b0*x[n] + b1*x[n-1] + ...
   //                        b10 b9 b8  b7 ....                 b1 b0
   static unsigned char coef[] = {0, 1, 3, 8, 12, 16, 12, 8, 3, 1, 0};   // N(0,3)
                        // filter coefitients (gaussian), requires sum(coef) = 65535/255 = 257
                        // same length as "buff"
   static unsigned int buff[FIR_BUF_LEN];
   unsigned int sum = 0;
   unsigned char 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+= buff[i] * coef[i];
   }

   // divide by 256
   //return sum/4;
   return (unsigned char)(sum >> 8);
}

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

void init_PWM1(void)
{
	TCCR0 = 0b01101011;
	
	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; 

}

// ========================== PROG================================
int main(void) 
{

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

	prumer = avg(read_ADC1()/4);

	while(1){
		//PWM1(prumer);
		PWM1(read_ADC1()/4); 
		//PWM1(avg(read_ADC1())/4);             
		//PWM1(gauss(read_ADC1()));
		PWM2(read_ADC2());                      
		                      
	}



}