#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/sleep.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <util/delay.h>
#include <avr/eeprom.h>
#include <stdio.h>

#include "defines.h"


inline void PwmOn(void)
{
	OCR1A = 255;
	OCR1B = 255;

	TCNT1H = 0;
	TCNT1L = 0;

	TCCR1A = 0b10110001;
	TCCR1B = 0b00001001;
}

inline void PwmOff(void)
{
	TCCR1A = 0;
	TCCR1B = 0;

//	clrbit(SND1PORT,SND1BIT);
//	clrbit(SND2PORT,SND2BIT);
}

inline void PwmNextSample(uint8_t sample)
{
	while( (TIFR&8) == 0)
		;
	TIFR |= 4;

	OCR1B = OCR1A = (uint16_t)sample;
}


void MeterOn(void)
{
	
	ADMUX = (1<<REFS1) | (1<<REFS0);
	ADCSRA = (1<<ADEN)  |
		(4<<ADPS0);

}


// Temp. samples

int8_t sample1, sample2;

// Buffer and pointers

int8_t buf[1024];
uint16_t head = 0, tail = 0;

int8_t blend_buf[32];
uint16_t blend_head = 0;



inline int16_t my_mulsu(int8_t s, uint8_t u)
{

	return s*((int8_t)u);
}


SIGNAL (SIG_OVERFLOW1) 
{
//	setbit(DDRB,0);
//	setbit(PORTB,0);

	// Put sample 2 into PWM

	OCR1B = OCR1A = sample2+128;
//	TIFR |= 4;

	// Get sample1 from ADC


	if (!getbit(PINA,4))
		ADMUX = (ADMUX & (~0x1F)) | (0); // ADC Nr
	else
	if (!getbit(PINA,5))
		ADMUX = (ADMUX & (~0x1F)) | (9); // ADC Nr
	else
		ADMUX = (ADMUX & (~0x1F)) | (11); // ADC Nr

	uint16_t result;
 	result = ADCL;			    		
 	result |= ((uint16_t)ADCH) << 8;

	if (!getbit(PINA,4))
		result -= 512; // Needed for ADC=0

	sample1 = (result) >> 2;
 
	ADCSRA |= (1<<ADSC); // Restart ADC


	// Signal processing

	blend_buf[blend_head] = buf[head>>3];

	buf[head>>3] = sample1;

	sample2 = buf[tail>>3];

	uint16_t dist;
	dist = ((head>>3) - (tail>>3)) & 1023;

	if (dist < 32)
	{
		sample2 = (my_mulsu(sample2, dist) + my_mulsu(blend_buf[(blend_head-dist)&31], 32-dist) ) >> 5; 
	}

	blend_head += 1;
	blend_head &= 31;

	head+=8;
	head &= 8191;


	// Pin


	if (getbit(PINA, 6))
		tail+= 13;  // sets pitch+, 8=none
	else
		tail+= 6;   // sets pitch-, 8=none

//	sample2 = (sample2>>1)+(sample1>>1);
//	tail += 2;

	tail &= 8191;

//	clrbit(PORTB,0);
}

int main(void)
{
	setbit(DDRD,4);
	setbit(DDRD,5);

//	clrbit(DDRC,2);
//	setbit(PORTC,2);

	clrbit(DDRA,4);
	setbit(PORTA,4);
	clrbit(DDRA,5);
	setbit(PORTA,5);
	clrbit(DDRA,6);
	setbit(PORTA,6);


//	MCUCR |= (8+4);
//	GICR |= 128;

	TIMSK |= 1<<TOIE1;

	sei();

	MeterOn();
	PwmOn();

again:

	

goto again;

    return(0);
}