#include <avr/io.h>
#include <math.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#ifndef F_CPU
	#define F_CPU 16000000L
	#include <util/delay.h> 
#endif
#define UART_UBRR_CALC(BAUD_,FREQ_) ((FREQ_)/((BAUD_)*16L)-1)
#define UART_BAUD_RATE      115200
#define UART_BUFFER            64
#define FREQUENCY 20000L
#define PI 3.14159
#define AMPLIFIER 2

/////////////////////////////////////////////////////////////////////////////////
//Init all variables
const float cosinus[] PROGMEM= 
{1.00000,0.99985,0.99939,0.99863,0.99756,0.99619,0.99452,0.99255,0.99027,
0.98769,0.98481,0.98163,0.97815,0.97437,0.97030,0.96593,0.96126,
0.95630,0.95106,0.94552,0.93969,0.93358,0.92718,0.92050,0.91355,
0.90631,0.89879,0.89101,0.88295,0.87462,0.86603,0.85717,0.84805,
0.83867,0.82904,0.81915,0.80902,0.79864,0.78801,0.77715,0.76604,
0.75471,0.74314,0.73135,0.71934,0.70711,0.69466,0.68200,0.66913,
0.65606,0.64279,0.62932,0.61566,0.60182,0.58779,0.57358,0.55919,
0.54464,0.52992,0.51504,0.50000,0.48481,0.46947,0.45399,0.43837,
0.42262,0.40674,0.39073,0.37461,0.35837,0.34202,0.32557,0.30902,
0.29237,0.27564,0.25882,0.24192,0.22495,0.20791,0.19081,0.17365,
0.15643,0.13917,0.12187,0.10453,0.08716,0.06976,0.05234,0.03490,
0.01745,0.00000,-0.01745,-0.03490,-0.05234,-0.06976,-0.08716,-0.10453,
-0.12187,-0.13917,-0.15643,-0.17365,-0.19081,-0.20791,-0.22495,-0.24192,
-0.25882,-0.27564,-0.29237,-0.30902,-0.32557,-0.34202,-0.35837,-0.37461,
-0.39073,-0.40674,-0.42262,-0.43837,-0.45399,-0.46947,-0.48481,-0.50000,
-0.51504,-0.52992,-0.54464,-0.55919,-0.57358,-0.58779,-0.60182,-0.61566,
-0.62932,-0.64279,-0.65606,-0.66913,-0.68200,-0.69466,-0.70711,-0.71934,
-0.73135,-0.74314,-0.75471,-0.76604,-0.77715,-0.78801,-0.79864,-0.80902,
-0.81915,-0.82904,-0.83867,-0.84805,-0.85717,-0.86603,-0.87462,-0.88295,
-0.89101,-0.89879,-0.90631,-0.91355,-0.92050,-0.92718,-0.93358,-0.93969,
-0.94552,-0.95106,-0.95630,-0.96126,-0.96593,-0.97030,-0.97437,-0.97815,
-0.98163,-0.98481,-0.98769,-0.99027,-0.99255,-0.99452,-0.99619,-0.99756,
-0.99863,-0.99939,-0.99985,-1.00000,-0.99985,-0.99939,-0.99863,-0.99756,
-0.99619,-0.99452,-0.99255,-0.99027,-0.98769,-0.98481,-0.98163,-0.97815,
-0.97437,-0.97030,-0.96593,-0.96126,-0.95630,-0.95106,-0.94552,-0.93969,
-0.93358,-0.92718,-0.92050,-0.91355,-0.90631,-0.89879,-0.89101,-0.88295,
-0.87462,-0.86603,-0.85717,-0.84805,-0.83867,-0.82904,-0.81915,-0.80902,
-0.79864,-0.78801,-0.77715,-0.76604,-0.75471,-0.74314,-0.73135,-0.71934,
-0.70711,-0.69466,-0.68200,-0.66913,-0.65606,-0.64279,-0.62932,-0.61566,
-0.60182,-0.58779,-0.57358,-0.55919,-0.54464,-0.52992,-0.51504,-0.50000,
-0.48481,-0.46947,-0.45399,-0.43837,-0.42262,-0.40674,-0.39073,-0.37461,
-0.35837,-0.34202,-0.32557,-0.30902,-0.29237,-0.27564,-0.25882,-0.24192,
-0.22495,-0.20791,-0.19081,-0.17365,-0.15643,-0.13917,-0.12187,-0.10453,
-0.08716,-0.06976,-0.05234,-0.03490,-0.01745,0.00000,0.01745,0.03490,
0.05234,0.06976,0.08716,0.10453,0.12187,0.13917,0.15643,0.17365,
0.19081,0.20791,0.22495,0.24192,0.25882,0.27564,0.29237,0.30902,
0.32557,0.34202,0.35837,0.37461,0.39073,0.40674,0.42262,0.43837,
0.45399,0.46947,0.48481,0.50000,0.51504,0.52992,0.54464,0.55919,
0.57358,0.58779,0.60182,0.61566,0.62932,0.64279,0.65606,0.66913,
0.68200,0.69466,0.70711,0.71934,0.73135,0.74314,0.75471,0.76604,
0.77715,0.78801,0.79864,0.80902,0.81915,0.82904,0.83867,0.84805,
0.85717,0.86603,0.87462,0.88295,0.89101,0.89879,0.90631,0.91355,
0.92050,0.92718,0.93358,0.93969,0.94552,0.95106,0.95630,0.96126,
0.96593,0.97030,0.97437,0.97815,0.98163,0.98481,0.98769,0.99027,
0.99255,0.99452,0.99619,0.99756,0.99863,0.99939,0.99985};
const float sinus[] PROGMEM= 
{0.00000,0.01745,0.03490,0.05234,0.06976,0.08716,0.10453,0.12187,0.13917,
0.15643,0.17365,0.19081,0.20791,0.22495,0.24192,0.25882,0.27564,
0.29237,0.30902,0.32557,0.34202,0.35837,0.37461,0.39073,0.40674,
0.42262,0.43837,0.45399,0.46947,0.48481,0.50000,0.51504,0.52992,
0.54464,0.55919,0.57358,0.58779,0.60182,0.61566,0.62932,0.64279,
0.65606,0.66913,0.68200,0.69466,0.70711,0.71934,0.73135,0.74314,
0.75471,0.76604,0.77715,0.78801,0.79864,0.80902,0.81915,0.82904,
0.83867,0.84805,0.85717,0.86603,0.87462,0.88295,0.89101,0.89879,
0.90631,0.91355,0.92050,0.92718,0.93358,0.93969,0.94552,0.95106,
0.95630,0.96126,0.96593,0.97030,0.97437,0.97815,0.98163,0.98481,
0.98769,0.99027,0.99255,0.99452,0.99619,0.99756,0.99863,0.99939,
0.99985,1.00000,0.99985,0.99939,0.99863,0.99756,0.99619,0.99452,
0.99255,0.99027,0.98769,0.98481,0.98163,0.97815,0.97437,0.97030,
0.96593,0.96126,0.95630,0.95106,0.94552,0.93969,0.93358,0.92718,
0.92050,0.91355,0.90631,0.89879,0.89101,0.88295,0.87462,0.86603,
0.85717,0.84805,0.83867,0.82904,0.81915,0.80902,0.79864,0.78801,
0.77715,0.76604,0.75471,0.74314,0.73135,0.71934,0.70711,0.69466,
0.68200,0.66913,0.65606,0.64279,0.62932,0.61566,0.60182,0.58779,
0.57358,0.55919,0.54464,0.52992,0.51504,0.50000,0.48481,0.46947,
0.45399,0.43837,0.42262,0.40674,0.39073,0.37461,0.35837,0.34202,
0.32557,0.30902,0.29237,0.27564,0.25882,0.24192,0.22495,0.20791,
0.19081,0.17365,0.15643,0.13917,0.12187,0.10453,0.08716,0.06976,
0.05234,0.03490,0.01745,0.00000,-0.01745,-0.03490,-0.05234,-0.06976,
-0.08716,-0.10453,-0.12187,-0.13917,-0.15643,-0.17365,-0.19081,-0.20791,
-0.22495,-0.24192,-0.25882,-0.27564,-0.29237,-0.30902,-0.32557,-0.34202,
-0.35837,-0.37461,-0.39073,-0.40674,-0.42262,-0.43837,-0.45399,-0.46947,
-0.48481,-0.50000,-0.51504,-0.52992,-0.54464,-0.55919,-0.57358,-0.58779,
-0.60182,-0.61566,-0.62932,-0.64279,-0.65606,-0.66913,-0.68200,-0.69466,
-0.70711,-0.71934,-0.73135,-0.74314,-0.75471,-0.76604,-0.77715,-0.78801,
-0.79864,-0.80902,-0.81915,-0.82904,-0.83867,-0.84805,-0.85717,-0.86603,
-0.87462,-0.88295,-0.89101,-0.89879,-0.90631,-0.91355,-0.92050,-0.92718,
-0.93358,-0.93969,-0.94552,-0.95106,-0.95630,-0.96126,-0.96593,-0.97030,
-0.97437,-0.97815,-0.98163,-0.98481,-0.98769,-0.99027,-0.99255,-0.99452,
-0.99619,-0.99756,-0.99863,-0.99939,-0.99985,-1.00000,-0.99985,-0.99939,
-0.99863,-0.99756,-0.99619,-0.99452,-0.99255,-0.99027,-0.98769,-0.98481,
-0.98163,-0.97815,-0.97437,-0.97030,-0.96593,-0.96126,-0.95630,-0.95106,
-0.94552,-0.93969,-0.93358,-0.92718,-0.92050,-0.91355,-0.90631,-0.89879,
-0.89101,-0.88295,-0.87462,-0.86603,-0.85717,-0.84805,-0.83867,-0.82904,
-0.81915,-0.80902,-0.79864,-0.78801,-0.77715,-0.76604,-0.75471,-0.74314,
-0.73135,-0.71934,-0.70711,-0.69466,-0.68200,-0.66913,-0.65606,-0.64279,
-0.62932,-0.61566,-0.60182,-0.58779,-0.57358,-0.55919,-0.54464,-0.52992,
-0.51504,-0.50000,-0.48481,-0.46947,-0.45399,-0.43837,-0.42262,-0.40674,
-0.39073,-0.37461,-0.35837,-0.34202,-0.32557,-0.30902,-0.29237,-0.27564,
-0.25882,-0.24192,-0.22495,-0.20791,-0.19081,-0.17365,-0.15643,-0.13917,
-0.12187,-0.10453,-0.08716,-0.06976,-0.05234,-0.03490,-0.01745};

float volatile factoramp = 4883;
float volatile amplitude =1.0;
float volatile real=0.0;
float volatile imag=0.0;
float volatile factor=0.0;
float volatile phase1 =0;

uint8_t volatile haveValue;
uint8_t uart_buffer[UART_BUFFER];
int8_t  uart_count  = 0; 

uint16_t phase0 =0;
uint16_t phase2 =0;
uint16_t phase3 =0; 
uint16_t phase4 =0;
uint16_t adc_value =0;
uint16_t prevCount;
uint16_t lowDuty;
uint16_t highDuty;
/////////////////////////////////////////////////////////////////////////////////
//Interrupts 
ISR(SIG_INPUT_CAPTURE1)
{
  	uint16_t nowCount = ICR1;

  if( TCCR1B & ( 1 << ICES1 ) ) {
    lowDuty = nowCount - prevCount;  // wir sind auf einer steigenden Flanke ->
                                     // d.h. der low Duty wurde gemessen
    TCCR1B &= ~( 1 << ICES1 );   // die nächste Flanke soll eine fallende sein
  }
  else {
  	TCNT1 = 0;
    highDuty = nowCount - prevCount;
    TCCR1B |= ( 1 << ICES1 );   // die nächste Flanke soll eine steigende sein

  }
  haveValue = 1;
  TIFR &= ~(1<<ICF1);
  prevCount = nowCount;

}

ISR(SIG_UART_RECV)
{ /* INTERRUPT: receive char from UART and save it the buffer
*/
    if (uart_count < UART_BUFFER) uart_buffer[uart_count++] = UDR;
} 
/////////////////////////////////////////////////////////////////////////////////
//Methodes
void input_capture_init()
{
	// ICF1 Interrupt free
	TIMSK |= (1<<ICF1);
	//// Timer frequency with 1 Prescaler
	TCCR1B |= (1<<CS10);
	TCCR1B &= ~(1<<ICES1);


}
inline void uart_init(uint8_t tx, uint8_t rx)
{
    uint16_t baudrate;
    baudrate = UART_BAUD_RATE/2;
    UBRRH    = (uint8_t) (UART_UBRR_CALC(baudrate,F_CPU)>>8);
    UBRRL    = (uint8_t) UART_UBRR_CALC(baudrate,F_CPU);
    UCSRA |= (1<<U2X);
    UCSRB |= (1<<RXCIE);
	UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0);
    if(tx) UCSRB |= (1<<TXEN);
    if(rx) UCSRB |= (1<<RXEN);
}
uint16_t adc(uint8_t admux){
	//AD-Wandler einschalten und Prescaler = 128 einstellen (enstpricht 115 khz Wandlertakt)

    ADCSRA = (1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
    //ADCSRA |=  (1<<ADFR);								//Conversion automatically	
	ADMUX   =  admux;									//Channel number
    ADMUX  |=  (1<<REFS1) | (1<<REFS0);					//Reference Intern 2.56V

    ADCSRA |=  (1<<ADSC); 			//Start conversion
   	while ( ADCSRA & (1<<ADSC) ) {
      		;   // auf Abschluss der Konvertierung warten
    	}
   	adc_value = ADCW;		    // Wandlungsergebnisse aufaddieren
                   // Summe durch vier teilen = arithm. Mittelwert
    ADCSRA &= ~(1<<ADEN);								//ADC reanables
    return adc_value;
}
void uart_puts(uint8_t *data)
{ /* send string per UART   */
    while(*data)
    {
        uart_putc(*data);
        data++;
    }
}
void uart_putf(float data)
{ 
	/* send float per UART BUG: unnötige Leerzeichen am Anfang?  */
    uint8_t buffer[7];
    uart_puts(dtostrf(data, 6, 3, buffer));
}
void uart_putc(uint8_t c)
{
    while (!(UCSRA & (1<<UDRE)));
    UDR = c;
}
void uart_puti(uint16_t data)
{
    uint8_t buffer[7];
    uart_puts(utoa(data, buffer, 10));
}
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
//Main programm
int main(void)
{
	// Port D: Ausgang für LED1 der Rest als Eingang
	DDRD = (1<<PD5); 								
	factor = (360.0*FREQUENCY/F_CPU);
    uart_init(1, 1);
	input_capture_init() ;
	uint16_t volatile i =0;
	sei();
	PORTD =  (1<<(PD5));
    while(1) // Unendlich lang wiederholen
    {	
		//Die Amplitude wird gemessen und berechnet.
		amplitude = (float) adc(0) * factoramp / 1000000.0 ;			//5V /1024 = 4883e-6
		if(haveValue)
		{
			haveValue = 0;
			// 0.45 = 360 (degree) 20000 (frequency) * 1/16 000 000 (clock) 
			phase1 = highDuty * factor;								
			phase2 = phase1;
			//Die trig. Werte werden berechnet
			memcpy_P(&real, &cosinus[phase2], sizeof(float));
			memcpy_P(&imag, &sinus[phase2], sizeof(float));
			//Real und Imag werden berechnet
			real *=  amplitude * AMPLIFIER;
			imag *=  amplitude * AMPLIFIER;
			//Daten werden übertragen
			uart_putf(real);
			uart_putc(';');
			uart_putf(imag);
			uart_putc('\n');
			uart_putc(13);
		}
    }
}